From 8efbb90ded6ac45a47647daeeca3a83e6bf5bb5d Mon Sep 17 00:00:00 2001 From: Butterscotch! Date: Sun, 26 Feb 2023 07:19:08 -0500 Subject: [PATCH] Convert AutoBone to Kotlin and extend PoseFrame data functionality (#581) --- .../java/dev/slimevr/autobone/AutoBone.java | 914 ------------------ .../java/dev/slimevr/autobone/AutoBone.kt | 758 +++++++++++++++ .../dev/slimevr/autobone/AutoBoneHandler.java | 436 --------- .../dev/slimevr/autobone/AutoBoneHandler.kt | 405 ++++++++ .../slimevr/autobone/AutoBoneListener.java | 26 - .../dev/slimevr/autobone/AutoBoneListener.kt | 21 + .../slimevr/autobone/AutoBoneProcessType.java | 31 - .../slimevr/autobone/AutoBoneProcessType.kt | 11 + .../autobone/AutoBoneTrainingStep.java | 108 --- .../slimevr/autobone/AutoBoneTrainingStep.kt | 25 + .../dev/slimevr/autobone/StatsCalculator.java | 51 - .../dev/slimevr/autobone/StatsCalculator.kt | 35 + .../autobone/errors/AutoBoneException.java | 28 - .../autobone/errors/AutoBoneException.kt | 14 + .../autobone/errors/BodyProportionError.java | 138 --- .../autobone/errors/BodyProportionError.kt | 127 +++ .../errors/FootHeightOffsetError.java | 57 -- .../autobone/errors/FootHeightOffsetError.kt | 52 + .../slimevr/autobone/errors/HeightError.java | 21 - .../slimevr/autobone/errors/HeightError.kt | 19 + .../autobone/errors/IAutoBoneError.java | 8 - .../slimevr/autobone/errors/IAutoBoneError.kt | 8 + .../autobone/errors/OffsetSlideError.java | 64 -- .../autobone/errors/OffsetSlideError.kt | 56 ++ .../autobone/errors/PositionError.java | 62 -- .../slimevr/autobone/errors/PositionError.kt | 58 ++ .../autobone/errors/PositionOffsetError.java | 81 -- .../autobone/errors/PositionOffsetError.kt | 59 ++ .../slimevr/autobone/errors/SlideError.java | 41 - .../dev/slimevr/autobone/errors/SlideError.kt | 44 + .../proportions/HardProportionLimiter.java | 36 - .../proportions/HardProportionLimiter.kt | 19 + .../errors/proportions/ProportionLimiter.java | 10 - .../errors/proportions/ProportionLimiter.kt | 8 + .../proportions/RangeProportionLimiter.java | 79 -- .../proportions/RangeProportionLimiter.kt | 65 ++ .../dev/slimevr/poserecorder/PoseFrameIO.java | 102 +- .../poserecorder/PoseFrameTracker.java | 22 +- .../slimevr/poserecorder/TrackerFrame.java | 86 +- .../poserecorder/TrackerFrameData.java | 16 - .../slimevr/poserecorder/TrackerFrameData.kt | 23 + .../posestreamer/PoseFrameStreamer.java | 4 + .../dev/slimevr/protocol/rpc/RPCHandler.java | 15 +- .../config/SkeletonConfigManager.java | 18 +- 44 files changed, 1993 insertions(+), 2268 deletions(-) delete mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBone.java create mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBone.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java create mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneListener.java create mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneListener.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.java create mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java create mode 100644 server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/StatsCalculator.java create mode 100644 server/src/main/java/dev/slimevr/autobone/StatsCalculator.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/HeightError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/HeightError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/PositionError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/PositionError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/SlideError.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/SlideError.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.kt delete mode 100644 server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.java create mode 100644 server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.kt delete mode 100644 server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.java create mode 100644 server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.kt diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBone.java b/server/src/main/java/dev/slimevr/autobone/AutoBone.java deleted file mode 100644 index 51ba8af89..000000000 --- a/server/src/main/java/dev/slimevr/autobone/AutoBone.java +++ /dev/null @@ -1,914 +0,0 @@ -package dev.slimevr.autobone; - -import com.jme3.math.FastMath; -import com.jme3.math.Vector3f; -import dev.slimevr.VRServer; -import dev.slimevr.autobone.errors.*; -import dev.slimevr.config.AutoBoneConfig; -import dev.slimevr.poserecorder.PoseFrameIO; -import dev.slimevr.poserecorder.PoseFrameTracker; -import dev.slimevr.poserecorder.PoseFrames; -import dev.slimevr.tracking.processor.BoneType; -import dev.slimevr.tracking.processor.HumanPoseManager; -import dev.slimevr.tracking.processor.TransformNode; -import dev.slimevr.tracking.processor.config.SkeletonConfigManager; -import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets; -import dev.slimevr.tracking.processor.skeleton.HumanSkeleton; -import dev.slimevr.tracking.trackers.TrackerRole; -import io.eiren.util.StringUtils; -import io.eiren.util.collections.FastList; -import io.eiren.util.logging.LogManager; -import org.apache.commons.lang3.tuple.Pair; - -import java.io.File; -import java.util.EnumMap; -import java.util.List; -import java.util.Map; -import java.util.Map.Entry; -import java.util.Random; -import java.util.function.BiConsumer; -import java.util.function.Consumer; -import java.util.function.Function; - - -public class AutoBone { - - private static final File saveDir = new File("AutoBone Recordings"); - private static final File loadDir = new File("Load AutoBone Recordings"); - - // This is filled by reloadConfigValues() - public final EnumMap offsets = new EnumMap<>( - BoneType.class - ); - - public final FastList adjustOffsets = new FastList<>( - new BoneType[] { - BoneType.HEAD, - BoneType.NECK, - BoneType.CHEST, - BoneType.WAIST, - BoneType.HIP, - - // This now works when using body proportion error! It's not the - // best still but it is somewhat functional - BoneType.LEFT_HIP, - - BoneType.LEFT_UPPER_LEG, - BoneType.LEFT_LOWER_LEG, - } - ); - - public final FastList heightOffsets = new FastList<>( - new BoneType[] { - BoneType.NECK, - BoneType.CHEST, - BoneType.WAIST, - BoneType.HIP, - - BoneType.LEFT_UPPER_LEG, - BoneType.RIGHT_UPPER_LEG, - BoneType.LEFT_LOWER_LEG, - BoneType.RIGHT_LOWER_LEG, - } - ); - - public final FastList legacyHeightConfigs = new FastList<>( - new SkeletonConfigOffsets[] { - SkeletonConfigOffsets.NECK, - SkeletonConfigOffsets.CHEST, - SkeletonConfigOffsets.WAIST, - SkeletonConfigOffsets.HIP, - SkeletonConfigOffsets.UPPER_LEG, - SkeletonConfigOffsets.LOWER_LEG, - } - ); - - public final EnumMap legacyConfigs = new EnumMap<>( - SkeletonConfigOffsets.class - ); - - protected final VRServer server; - - // #region Error functions - public SlideError slideError = new SlideError(); - public OffsetSlideError offsetSlideError = new OffsetSlideError(); - public FootHeightOffsetError footHeightOffsetError = new FootHeightOffsetError(); - public BodyProportionError bodyProportionError = new BodyProportionError(); - public HeightError heightError = new HeightError(); - public PositionError positionError = new PositionError(); - public PositionOffsetError positionOffsetError = new PositionOffsetError(); - // #endregion - - private final Random rand = new Random(); - - private final AutoBoneConfig config; - - public AutoBone(VRServer server) { - this.config = server.getConfigManager().getVrConfig().getAutoBone(); - this.server = server; - reloadConfigValues(); - } - - // Mean square error function - protected static float errorFunc(float errorDeriv) { - return 0.5f * (errorDeriv * errorDeriv); - } - - public static File getLoadDir() { - return loadDir; - } - - public float computeBoneOffset( - BoneType bone, - Function getOffset - ) { - return switch (bone) { - case HEAD -> getOffset.apply(SkeletonConfigOffsets.HEAD); - case NECK -> getOffset.apply(SkeletonConfigOffsets.NECK); - case CHEST -> getOffset.apply(SkeletonConfigOffsets.CHEST); - case WAIST -> getOffset.apply(SkeletonConfigOffsets.WAIST); - case HIP -> getOffset.apply(SkeletonConfigOffsets.HIP); - case LEFT_HIP, RIGHT_HIP -> getOffset.apply(SkeletonConfigOffsets.HIPS_WIDTH) - / 2f; - case LEFT_UPPER_LEG, RIGHT_UPPER_LEG -> getOffset - .apply(SkeletonConfigOffsets.UPPER_LEG); - case LEFT_LOWER_LEG, RIGHT_LOWER_LEG -> getOffset - .apply(SkeletonConfigOffsets.LOWER_LEG); - default -> -1f; - }; - - } - - public void reloadConfigValues() { - reloadConfigValues(null); - } - - public void reloadConfigValues(List trackers) { - // Remove all previous values - offsets.clear(); - - // Get current or default skeleton configs - HumanPoseManager skeleton = getHumanPoseManager(); - Function getOffset = skeleton != null - ? skeleton::getOffset - : new SkeletonConfigManager(false)::getOffset; - - for (BoneType bone : adjustOffsets) { - float offset = computeBoneOffset(bone, getOffset); - if (offset > 0f) { - offsets.put(bone, offset); - } - } - } - - public Vector3f getBoneDirection( - HumanPoseManager skeleton, - BoneType node, - boolean rightSide, - Vector3f buffer - ) { - if (buffer == null) { - buffer = new Vector3f(); - } - - switch (node) { - case LEFT_HIP, RIGHT_HIP -> node = rightSide ? BoneType.RIGHT_HIP : BoneType.LEFT_HIP; - case LEFT_UPPER_LEG, RIGHT_UPPER_LEG -> node = rightSide - ? BoneType.RIGHT_UPPER_LEG - : BoneType.LEFT_UPPER_LEG; - case LEFT_LOWER_LEG, RIGHT_LOWER_LEG -> node = rightSide - ? BoneType.RIGHT_LOWER_LEG - : BoneType.LEFT_LOWER_LEG; - } - - TransformNode relevantTransform = skeleton.getTailNodeOfBone(node); - return relevantTransform.worldTransform - .getTranslation() - .subtract(relevantTransform.getParent().worldTransform.getTranslation(), buffer) - .normalizeLocal(); - } - - public float getDotProductDiff( - HumanPoseManager skeleton1, - HumanPoseManager skeleton2, - BoneType node, - boolean rightSide, - Vector3f offset - ) { - Vector3f normalizedOffset = offset.normalize(); - - Vector3f boneRotation = new Vector3f(); - getBoneDirection(skeleton1, node, rightSide, boneRotation); - float dot1 = normalizedOffset.dot(boneRotation); - - getBoneDirection(skeleton2, node, rightSide, boneRotation); - float dot2 = normalizedOffset.dot(boneRotation); - - return dot2 - dot1; - } - - /** - * A simple utility method to get the {@link HumanSkeleton} from the - * {@link VRServer} - * - * @return The {@link HumanSkeleton} associated with the {@link VRServer}, - * or null if there is none available - * @see {@link VRServer}, {@link HumanSkeleton} - */ - private HumanPoseManager getHumanPoseManager() { - return server != null ? server.humanPoseManager : null; - } - - public void applyAndSaveConfig() { - if (!applyAndSaveConfig(getHumanPoseManager())) { - // Unable to apply to skeleton, save directly - // saveConfigs(); - } - } - - public boolean applyConfig( - BiConsumer configConsumer, - Map offsets - ) { - if (configConsumer == null || offsets == null) { - return false; - } - - try { - Float headOffset = offsets.get(BoneType.HEAD); - if (headOffset != null) { - configConsumer.accept(SkeletonConfigOffsets.HEAD, headOffset); - } - Float neckOffset = offsets.get(BoneType.NECK); - if (neckOffset != null) { - configConsumer.accept(SkeletonConfigOffsets.NECK, neckOffset); - } - - Float chestOffset = offsets.get(BoneType.CHEST); - Float waistOffset = offsets.get(BoneType.WAIST); - Float hipOffset = offsets.get(BoneType.HIP); - if (chestOffset != null) { - configConsumer - .accept(SkeletonConfigOffsets.CHEST, chestOffset); - } - if (waistOffset != null) { - configConsumer.accept(SkeletonConfigOffsets.WAIST, waistOffset); - } - if (hipOffset != null) { - configConsumer.accept(SkeletonConfigOffsets.HIP, hipOffset); - } - - Float hipWidthOffset = offsets.get(BoneType.LEFT_HIP); - if (hipWidthOffset == null) { - hipWidthOffset = offsets.get(BoneType.RIGHT_HIP); - } - if (hipWidthOffset != null) { - configConsumer - .accept(SkeletonConfigOffsets.HIPS_WIDTH, hipWidthOffset * 2f); - } - - Float upperLegOffset = offsets.get(BoneType.LEFT_UPPER_LEG); - if (upperLegOffset == null) { - upperLegOffset = offsets.get(BoneType.RIGHT_UPPER_LEG); - } - Float lowerLegOffset = offsets.get(BoneType.LEFT_LOWER_LEG); - if (lowerLegOffset == null) { - lowerLegOffset = offsets.get(BoneType.RIGHT_LOWER_LEG); - } - - if (upperLegOffset != null) { - configConsumer - .accept(SkeletonConfigOffsets.UPPER_LEG, upperLegOffset); - } - if (lowerLegOffset != null) { - configConsumer.accept(SkeletonConfigOffsets.LOWER_LEG, lowerLegOffset); - } - - return true; - } catch (Exception e) { - return false; - } - } - - public boolean applyConfig(BiConsumer configConsumer) { - return applyConfig(configConsumer, offsets); - } - - public boolean applyConfig( - Map skeletonConfig, - Map offsets - ) { - if (skeletonConfig == null) { - return false; - } - - return applyConfig(skeletonConfig::put, offsets); - } - - public boolean applyConfig(Map skeletonConfig) { - return applyConfig(skeletonConfig, offsets); - } - - public boolean applyConfig( - HumanPoseManager humanPoseManager, - Map offsets - ) { - if (humanPoseManager == null) { - return false; - } - - return applyConfig(humanPoseManager::setOffset, offsets); - } - - public boolean applyConfig(HumanPoseManager humanPoseManager) { - return applyConfig(humanPoseManager, offsets); - } - - public boolean applyAndSaveConfig(HumanPoseManager humanPoseManager) { - if (humanPoseManager == null) { - return false; - } - - if (!applyConfig(humanPoseManager)) - return false; - - humanPoseManager.saveConfig(); - server.getConfigManager().saveConfig(); - - LogManager.info("[AutoBone] Configured skeleton bone lengths"); - return true; - } - - public Float getConfig(BoneType config) { - return offsets.get(config); - } - - public float sumSelectConfigs( - List selection, - Function configs - ) { - float sum = 0f; - - for (T config : selection) { - Float length = configs.apply(config); - if (length != null) { - sum += length; - } - } - - return sum; - } - - public float sumSelectConfigs( - List selection, - Map configs - ) { - return sumSelectConfigs(selection, configs::get); - } - - public float sumSelectConfigs( - List selection, - HumanPoseManager humanPoseManager - ) { - return sumSelectConfigs(selection, humanPoseManager::getOffset); - } - - public float getLengthSum(Map configs) { - return getLengthSum(configs, null); - } - - public float getLengthSum( - Map configs, - Map configsAlt - ) { - float length = 0f; - - if (configsAlt != null) { - for (Entry config : configsAlt.entrySet()) { - // If there isn't a duplicate config - if (!configs.containsKey(config.getKey())) { - length += config.getValue(); - } - } - } - - for (Float boneLength : configs.values()) { - length += boneLength; - } - - return length; - } - - public float getTargetHeight(PoseFrames frames) { - float targetHeight; - // Get the current skeleton from the server - HumanPoseManager humanPoseManager = getHumanPoseManager(); - if (humanPoseManager != null) { - // If there is a skeleton available, calculate the target height - // from its configs - targetHeight = sumSelectConfigs(legacyHeightConfigs, humanPoseManager); - LogManager - .warning( - "[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " - + targetHeight - ); - } else { - // Otherwise if there is no skeleton available, attempt to get the - // max HMD height from the recording - float hmdHeight = frames.getMaxHmdHeight(); - if (hmdHeight <= 0.50f) { - LogManager - .warning( - "[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): " - + hmdHeight - ); - } else { - LogManager.info("[AutoBone] Max headset height detected: " + hmdHeight); - } - - // Estimate target height from HMD height - targetHeight = hmdHeight; - } - - return targetHeight; - } - - public AutoBoneResults processFrames(PoseFrames frames, Consumer epochCallback) - throws AutoBoneException { - return processFrames(frames, -1f, epochCallback); - } - - public AutoBoneResults processFrames( - PoseFrames frames, - float targetHeight, - Consumer epochCallback - ) throws AutoBoneException { - return processFrames(frames, true, targetHeight, epochCallback); - } - - public AutoBoneResults processFrames( - PoseFrames frames, - boolean calcInitError, - float targetHeight, - Consumer epochCallback - ) throws AutoBoneException { - final int frameCount = frames.getMaxFrameCount(); - - final PoseFrames frames1 = new PoseFrames(frames); - final PoseFrames frames2 = new PoseFrames(frames); - - List trackers1 = frames1.getTrackers(); - List trackers2 = frames2.getTrackers(); - - // Reload configs and detect chest tracker from the first frame - reloadConfigValues(trackers1); - - final HumanPoseManager skeleton1 = new HumanPoseManager( - trackers1 - ); - final HumanPoseManager skeleton2 = new HumanPoseManager( - trackers2 - ); - - EnumMap intermediateOffsets = new EnumMap<>( - offsets - ); - - AutoBoneTrainingStep trainingStep = new AutoBoneTrainingStep( - targetHeight, - skeleton1, - skeleton2, - frames, - intermediateOffsets - ); - - skeleton1.setLegTweaksEnabled(false); - skeleton2.setLegTweaksEnabled(false); - - // If target height isn't specified, auto-detect - if (targetHeight < 0f) { - targetHeight = getTargetHeight(frames); - } - - StatsCalculator errorStats = new StatsCalculator(); - - // Epoch loop, each epoch is one full iteration over the full dataset - for (int epoch = calcInitError ? -1 : 0; epoch < this.config.numEpochs; epoch++) { - float adjustRate = epoch >= 0 - ? (this.config.initialAdjustRate - * FastMath.pow(this.config.adjustRateMultiplier, epoch)) - : 0f; - - int[] randomFrameIndices = null; - if (config.randomizeFrameOrder) { - randomFrameIndices = new int[frameCount]; - - int zeroPos = -1; - for (int i = 0; i < frameCount; i++) { - int index = rand.nextInt(frameCount); - - if (i > 0) { - while (index == zeroPos || randomFrameIndices[index] > 0) { - index = rand.nextInt(frameCount); - } - } else { - zeroPos = index; - } - - randomFrameIndices[index] = i; - } - } - - // Iterate over the frames using a cursor and an offset for - // comparing frames a - // certain number of frames apart - for ( - int cursorOffset = this.config.minDataDistance; - cursorOffset <= this.config.maxDataDistance - && cursorOffset < frameCount; - cursorOffset++ - ) { - for ( - int frameCursor = 0; frameCursor < frameCount - cursorOffset; - frameCursor += config.cursorIncrement - ) { - int frameCursor2 = frameCursor + cursorOffset; - - applyConfig(skeleton1); - applyConfig(skeleton2); - - if (config.randomizeFrameOrder) { - trainingStep - .setCursors( - randomFrameIndices[frameCursor], - randomFrameIndices[frameCursor2] - ); - } else { - trainingStep.setCursors(frameCursor, frameCursor2); - } - - frames1.setCursors(trainingStep.getCursor1()); - frames2.setCursors(trainingStep.getCursor2()); - - skeleton1.update(); - skeleton2.update(); - - float totalLength = getLengthSum(offsets); - float curHeight = sumSelectConfigs(heightOffsets, offsets); - trainingStep.setCurrentHeight(curHeight); - - float errorDeriv = getErrorDeriv(trainingStep); - float error = errorFunc(errorDeriv); - - // In case of fire - if (Float.isNaN(error) || Float.isInfinite(error)) { - // Extinguish - LogManager - .warning( - "[AutoBone] Error value is invalid, resetting variables to recover" - ); - reloadConfigValues(trackers1); - - // Reset error sum values - errorStats.reset(); - - // Continue on new data - continue; - } - - // Store the error count for logging purposes - errorStats.addValue(errorDeriv); - - float adjustVal = error * adjustRate; - - // If there is no adjustment whatsoever, skip this - if (adjustVal == 0f) { - continue; - } - - Vector3f slideLeft = skeleton2 - .getComputedTracker(TrackerRole.LEFT_FOOT).position - .subtract( - skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position - ); - - Vector3f slideRight = skeleton2 - .getComputedTracker(TrackerRole.RIGHT_FOOT).position - .subtract( - skeleton1 - .getComputedTracker(TrackerRole.RIGHT_FOOT).position - ); - - intermediateOffsets.putAll(offsets); - for (Entry entry : offsets.entrySet()) { - // Skip adjustment if the epoch is before starting (for - // logging only) - if (epoch < 0) { - break; - } - - float originalLength = entry.getValue(); - boolean isHeightVar = heightOffsets.contains(entry.getKey()); - - float leftDotProduct = getDotProductDiff( - skeleton1, - skeleton2, - entry.getKey(), - false, - slideLeft - ); - - float rightDotProduct = getDotProductDiff( - skeleton1, - skeleton2, - entry.getKey(), - true, - slideRight - ); - - float dotLength = originalLength - * ((leftDotProduct + rightDotProduct) / 2f); - - // Scale by the ratio for smooth adjustment and more - // stable results - float curAdjustVal = (adjustVal * -dotLength) / totalLength; - float newLength = originalLength + curAdjustVal; - - // No small or negative numbers!!! Bad algorithm! - if (newLength < 0.01f) { - continue; - } - - // Apply new offset length - intermediateOffsets.put(entry.getKey(), newLength); - applyConfig(skeleton1, intermediateOffsets); - applyConfig(skeleton2, intermediateOffsets); - - // Update the skeleton poses for the new offset length - skeleton1.update(); - skeleton2.update(); - - float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight; - trainingStep.setCurrentHeight(newHeight); - - float newErrorDeriv = getErrorDeriv(trainingStep); - - if (newErrorDeriv < errorDeriv) { - entry.setValue(newLength); - } - - // Reset the length to minimize bias in other variables, - // it's applied later - intermediateOffsets.put(entry.getKey(), originalLength); - applyConfig(skeleton1, intermediateOffsets); - applyConfig(skeleton2, intermediateOffsets); - } - - if (config.scaleEachStep) { - float stepHeight = sumSelectConfigs(heightOffsets, offsets); - - if (stepHeight > 0f) { - float stepHeightDiff = targetHeight - stepHeight; - for (Entry entry : offsets.entrySet()) { - // Only height variables - if ( - entry.getKey() == BoneType.NECK - || !heightOffsets.contains(entry.getKey()) - ) - continue; - - float length = entry.getValue(); - - // Multiply the diff by the length to height - // ratio - float adjVal = stepHeightDiff * (length / stepHeight); - - // Scale the length to fit the target height - entry.setValue(Math.max(length + (adjVal / 2f), 0.01f)); - } - } - } - } - } - - // Calculate average error over the epoch - if ( - epoch <= 0 - || epoch >= (config.numEpochs - 1) - || (epoch + 1) % config.printEveryNumEpochs == 0 - ) { - LogManager - .info( - "[AutoBone] Epoch " - + (epoch + 1) - + " average error: " - + errorStats.getMean() - + " (SD " - + errorStats.getStandardDeviation() - + ")" - ); - } - - applyConfig(legacyConfigs); - if (epochCallback != null) { - epochCallback - .accept(new Epoch(epoch + 1, this.config.numEpochs, errorStats, legacyConfigs)); - } - } - - float finalHeight = sumSelectConfigs(heightOffsets, offsets); - LogManager - .info( - "[AutoBone] Target height: " - + targetHeight - + " New height: " - + finalHeight - ); - - return new AutoBoneResults(finalHeight, targetHeight, errorStats, legacyConfigs); - } - - protected float getErrorDeriv(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - float sumError = 0f; - - if (this.config.slideErrorFactor > 0f) { - sumError += slideError.getStepError(trainingStep) * this.config.slideErrorFactor; - } - - if (this.config.offsetSlideErrorFactor > 0f) { - sumError += offsetSlideError.getStepError(trainingStep) - * this.config.offsetSlideErrorFactor; - } - - if (this.config.footHeightOffsetErrorFactor > 0f) { - sumError += footHeightOffsetError.getStepError(trainingStep) - * this.config.footHeightOffsetErrorFactor; - } - - if (this.config.bodyProportionErrorFactor > 0f) { - sumError += bodyProportionError.getStepError(trainingStep) - * this.config.bodyProportionErrorFactor; - } - - if (this.config.heightErrorFactor > 0f) { - sumError += heightError.getStepError(trainingStep) * this.config.heightErrorFactor; - } - - if (this.config.positionErrorFactor > 0f) { - sumError += positionError.getStepError(trainingStep) - * this.config.positionErrorFactor; - } - - if (this.config.positionOffsetErrorFactor > 0f) { - sumError += positionOffsetError.getStepError(trainingStep) - * this.config.positionOffsetErrorFactor; - } - - return sumError; - } - - public String getLengthsString() { - final StringBuilder configInfo = new StringBuilder(); - this.offsets.forEach((key, value) -> { - if (configInfo.length() > 0) { - configInfo.append(", "); - } - - configInfo - .append(key.toString()) - .append(": ") - .append(StringUtils.prettyNumber(value * 100f, 2)); - }); - - return configInfo.toString(); - } - - public void saveRecording(PoseFrames frames, File recordingFile) { - if (saveDir.isDirectory() || saveDir.mkdirs()) { - LogManager - .info("[AutoBone] Exporting frames to \"" + recordingFile.getPath() + "\"..."); - if (PoseFrameIO.writeToFile(recordingFile, frames)) { - LogManager - .info( - "[AutoBone] Done exporting! Recording can be found at \"" - + recordingFile.getPath() - + "\"." - ); - } else { - LogManager - .severe( - "[AutoBone] Failed to export the recording to \"" - + recordingFile.getPath() - + "\"." - ); - } - } else { - LogManager - .severe( - "[AutoBone] Failed to create the recording directory \"" - + saveDir.getPath() - + "\"." - ); - } - } - - public void saveRecording(PoseFrames frames, String recordingFileName) { - saveRecording(frames, new File(saveDir, recordingFileName)); - } - - public void saveRecording(PoseFrames frames) { - File recordingFile; - int recordingIndex = 1; - do { - recordingFile = new File(saveDir, "ABRecording" + recordingIndex++ + ".pfr"); - } while (recordingFile.exists()); - - saveRecording(frames, recordingFile); - } - - public List> loadRecordings() { - List> recordings = new FastList<>(); - if (loadDir.isDirectory()) { - File[] files = loadDir.listFiles(); - if (files != null) { - for (File file : files) { - if ( - file.isFile() - && org.apache.commons.lang3.StringUtils - .endsWithIgnoreCase(file.getName(), ".pfr") - ) { - LogManager - .info( - "[AutoBone] Detected recording at \"" - + file.getPath() - + "\", loading frames..." - ); - PoseFrames frames = PoseFrameIO.readFromFile(file); - - if (frames == null) { - LogManager - .severe("Reading frames from \"" + file.getPath() + "\" failed..."); - } else { - recordings.add(Pair.of(file.getName(), frames)); - } - } - } - } - } - - return recordings; - } - - public AutoBoneConfig getConfig() { - return config; - } - - public class Epoch { - - public final int epoch; - public final int totalEpochs; - public final StatsCalculator epochError; - public final EnumMap configValues; - - public Epoch( - int epoch, - int totalEpochs, - StatsCalculator epochError, - EnumMap configValues - ) { - this.epoch = epoch; - this.totalEpochs = totalEpochs; - this.epochError = epochError; - this.configValues = configValues; - } - - @Override - public String toString() { - return "Epoch: " + epoch + ", Epoch Error: " + epochError; - } - } - - public class AutoBoneResults { - - public final float finalHeight; - public final float targetHeight; - public final StatsCalculator epochError; - public final EnumMap configValues; - - public AutoBoneResults( - float finalHeight, - float targetHeight, - StatsCalculator epochError, - EnumMap configValues - ) { - this.finalHeight = finalHeight; - this.targetHeight = targetHeight; - this.epochError = epochError; - this.configValues = configValues; - } - - public float getHeightDifference() { - return FastMath.abs(targetHeight - finalHeight); - } - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBone.kt b/server/src/main/java/dev/slimevr/autobone/AutoBone.kt new file mode 100644 index 000000000..38c87754c --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/AutoBone.kt @@ -0,0 +1,758 @@ +package dev.slimevr.autobone + +import com.jme3.math.FastMath +import com.jme3.math.Vector3f +import dev.slimevr.VRServer +import dev.slimevr.autobone.errors.AutoBoneException +import dev.slimevr.autobone.errors.BodyProportionError +import dev.slimevr.autobone.errors.FootHeightOffsetError +import dev.slimevr.autobone.errors.HeightError +import dev.slimevr.autobone.errors.OffsetSlideError +import dev.slimevr.autobone.errors.PositionError +import dev.slimevr.autobone.errors.PositionOffsetError +import dev.slimevr.autobone.errors.SlideError +import dev.slimevr.config.AutoBoneConfig +import dev.slimevr.poserecorder.PoseFrameIO +import dev.slimevr.poserecorder.PoseFrameTracker +import dev.slimevr.poserecorder.PoseFrames +import dev.slimevr.tracking.processor.BoneType +import dev.slimevr.tracking.processor.HumanPoseManager +import dev.slimevr.tracking.processor.config.SkeletonConfigManager +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import dev.slimevr.tracking.trackers.TrackerRole +import io.eiren.util.StringUtils +import io.eiren.util.collections.FastList +import io.eiren.util.logging.LogManager +import org.apache.commons.lang3.tuple.Pair +import java.io.File +import java.util.* +import java.util.function.BiConsumer +import java.util.function.Consumer +import java.util.function.Function + +class AutoBone(server: VRServer) { + // This is filled by reloadConfigValues() + val offsets = EnumMap( + BoneType::class.java + ) + val adjustOffsets = FastList( + arrayOf( + BoneType.HEAD, + BoneType.NECK, + BoneType.CHEST, + BoneType.WAIST, + BoneType.HIP, // This now works when using body proportion error! It's not the + // best still, but it is somewhat functional + BoneType.LEFT_HIP, + BoneType.LEFT_UPPER_LEG, + BoneType.LEFT_LOWER_LEG + ) + ) + val heightOffsets = FastList( + arrayOf( + BoneType.NECK, + BoneType.CHEST, + BoneType.WAIST, + BoneType.HIP, + BoneType.LEFT_UPPER_LEG, + BoneType.RIGHT_UPPER_LEG, + BoneType.LEFT_LOWER_LEG, + BoneType.RIGHT_LOWER_LEG + ) + ) + + val legacyConfigs = EnumMap( + SkeletonConfigOffsets::class.java + ) + + private val server: VRServer + + // #region Error functions + var slideError = SlideError() + var offsetSlideError = OffsetSlideError() + var footHeightOffsetError = FootHeightOffsetError() + var bodyProportionError = BodyProportionError() + var heightError = HeightError() + var positionError = PositionError() + var positionOffsetError = PositionOffsetError() + + // #endregion + private val rand = Random() + + val config: AutoBoneConfig + + init { + config = server.configManager.vrConfig.autoBone + this.server = server + reloadConfigValues() + } + + fun computeBoneOffset( + bone: BoneType?, + getOffset: Function, + ): Float { + return when (bone) { + BoneType.HEAD -> getOffset.apply(SkeletonConfigOffsets.HEAD) + BoneType.NECK -> getOffset.apply(SkeletonConfigOffsets.NECK) + BoneType.CHEST -> getOffset.apply(SkeletonConfigOffsets.CHEST) + BoneType.WAIST -> getOffset.apply(SkeletonConfigOffsets.WAIST) + BoneType.HIP -> getOffset.apply(SkeletonConfigOffsets.HIP) + BoneType.LEFT_HIP, BoneType.RIGHT_HIP -> ( + getOffset.apply(SkeletonConfigOffsets.HIPS_WIDTH) / + 2f + ) + + BoneType.LEFT_UPPER_LEG, BoneType.RIGHT_UPPER_LEG -> + getOffset + .apply(SkeletonConfigOffsets.UPPER_LEG) + + BoneType.LEFT_LOWER_LEG, BoneType.RIGHT_LOWER_LEG -> + getOffset + .apply(SkeletonConfigOffsets.LOWER_LEG) + + else -> -1f + } + } + + fun reloadConfigValues(trackers: List? = null) { + // Remove all previous values + offsets.clear() + + // Get current or default skeleton configs + val skeleton = humanPoseManager + val getOffset: Function = + if (skeleton != null) { + Function { key: SkeletonConfigOffsets? -> skeleton.getOffset(key) } + } else { + Function { config: SkeletonConfigOffsets? -> + SkeletonConfigManager(false).getOffset(config) + } + } + for (bone in adjustOffsets) { + val offset = computeBoneOffset(bone, getOffset) + if (offset > 0f) { + offsets[bone] = offset + } + } + } + + fun getBoneDirection( + skeleton: HumanPoseManager, + node: BoneType, + rightSide: Boolean, + buffer: Vector3f?, + ): Vector3f { + var node = node + var buffer = buffer + if (buffer == null) { + buffer = Vector3f() + } + when (node) { + BoneType.LEFT_HIP, BoneType.RIGHT_HIP -> node = if (rightSide) BoneType.RIGHT_HIP else BoneType.LEFT_HIP + BoneType.LEFT_UPPER_LEG, BoneType.RIGHT_UPPER_LEG -> + node = + if (rightSide) BoneType.RIGHT_UPPER_LEG else BoneType.LEFT_UPPER_LEG + + BoneType.LEFT_LOWER_LEG, BoneType.RIGHT_LOWER_LEG -> + node = + if (rightSide) BoneType.RIGHT_LOWER_LEG else BoneType.LEFT_LOWER_LEG + + else -> { + // Node is correct + } + } + val relevantTransform = skeleton.getTailNodeOfBone(node) + return relevantTransform.worldTransform + .translation + .subtract(relevantTransform.parent.worldTransform.translation, buffer) + .normalizeLocal() + } + + fun getDotProductDiff( + skeleton1: HumanPoseManager, + skeleton2: HumanPoseManager, + node: BoneType, + rightSide: Boolean, + offset: Vector3f, + ): Float { + val normalizedOffset = offset.normalize() + val boneRotation = Vector3f() + getBoneDirection(skeleton1, node, rightSide, boneRotation) + val dot1 = normalizedOffset.dot(boneRotation) + getBoneDirection(skeleton2, node, rightSide, boneRotation) + val dot2 = normalizedOffset.dot(boneRotation) + return dot2 - dot1 + } + + private val humanPoseManager: HumanPoseManager? + /** + * A simple utility method to get the [HumanSkeleton] from the + * [VRServer] + * + * @return The [HumanSkeleton] associated with the [VRServer], + * or null if there is none available + * @see {@link VRServer}, {@link HumanSkeleton} + */ + get() = server.humanPoseManager + + fun applyConfig( + configConsumer: BiConsumer, + offsets: Map = this.offsets, + ): Boolean { + return try { + val headOffset = offsets[BoneType.HEAD] + if (headOffset != null) { + configConsumer.accept(SkeletonConfigOffsets.HEAD, headOffset) + } + val neckOffset = offsets[BoneType.NECK] + if (neckOffset != null) { + configConsumer.accept(SkeletonConfigOffsets.NECK, neckOffset) + } + val chestOffset = offsets[BoneType.CHEST] + val waistOffset = offsets[BoneType.WAIST] + val hipOffset = offsets[BoneType.HIP] + if (chestOffset != null) { + configConsumer + .accept(SkeletonConfigOffsets.CHEST, chestOffset) + } + if (waistOffset != null) { + configConsumer.accept(SkeletonConfigOffsets.WAIST, waistOffset) + } + if (hipOffset != null) { + configConsumer.accept(SkeletonConfigOffsets.HIP, hipOffset) + } + var hipWidthOffset = offsets[BoneType.LEFT_HIP] + if (hipWidthOffset == null) { + hipWidthOffset = offsets[BoneType.RIGHT_HIP] + } + if (hipWidthOffset != null) { + configConsumer + .accept(SkeletonConfigOffsets.HIPS_WIDTH, hipWidthOffset * 2f) + } + var upperLegOffset = offsets[BoneType.LEFT_UPPER_LEG] + if (upperLegOffset == null) { + upperLegOffset = offsets[BoneType.RIGHT_UPPER_LEG] + } + var lowerLegOffset = offsets[BoneType.LEFT_LOWER_LEG] + if (lowerLegOffset == null) { + lowerLegOffset = offsets[BoneType.RIGHT_LOWER_LEG] + } + if (upperLegOffset != null) { + configConsumer + .accept(SkeletonConfigOffsets.UPPER_LEG, upperLegOffset) + } + if (lowerLegOffset != null) { + configConsumer.accept(SkeletonConfigOffsets.LOWER_LEG, lowerLegOffset) + } + true + } catch (e: Exception) { + false + } + } + + fun applyConfig( + skeletonConfig: MutableMap, + offsets: Map = this.offsets, + ): Boolean { + return applyConfig({ key: SkeletonConfigOffsets, value: Float -> skeletonConfig[key] = value }, offsets) + } + + fun applyConfig( + humanPoseManager: HumanPoseManager, + offsets: Map = this.offsets, + ): Boolean { + return applyConfig({ key: SkeletonConfigOffsets?, newLength: Float? -> + humanPoseManager.setOffset( + key, + newLength + ) + }, offsets) + } + + @JvmOverloads + fun applyAndSaveConfig(humanPoseManager: HumanPoseManager? = this.humanPoseManager): Boolean { + if (humanPoseManager == null) return false + if (!applyConfig(humanPoseManager)) return false + humanPoseManager.saveConfig() + server.configManager.saveConfig() + LogManager.info("[AutoBone] Configured skeleton bone lengths") + return true + } + + fun getConfig(config: BoneType): Float? { + return offsets[config] + } + + fun sumSelectConfigs( + selection: List, + configs: Function, + ): Float { + var sum = 0f + for (config in selection) { + val length = configs.apply(config) + if (length != null) { + sum += length + } + } + return sum + } + + fun sumSelectConfigs( + selection: List, + configs: Map, + ): Float { + return sumSelectConfigs(selection) { key: T -> configs[key] } + } + + fun sumSelectConfigs( + selection: List, + humanPoseManager: HumanPoseManager, + ): Float { + return sumSelectConfigs(selection) { key: SkeletonConfigOffsets? -> humanPoseManager.getOffset(key) } + } + + fun getLengthSum(configs: Map): Float { + return getLengthSum(configs, null) + } + + fun getLengthSum( + configs: Map, + configsAlt: Map?, + ): Float { + var length = 0f + if (configsAlt != null) { + for ((key, value) in configsAlt) { + // If there isn't a duplicate config + if (!configs.containsKey(key)) { + length += value + } + } + } + for (boneLength in configs.values) { + length += boneLength + } + return length + } + + fun getTargetHeight(frames: PoseFrames): Float { + val targetHeight: Float + // Get the current skeleton from the server + val humanPoseManager = humanPoseManager + if (humanPoseManager != null) { + // If there is a skeleton available, calculate the target height + // from its configs + targetHeight = humanPoseManager.userHeightFromConfig + LogManager + .warning( + "[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): $targetHeight" + ) + } else { + // Otherwise if there is no skeleton available, attempt to get the + // max HMD height from the recording + val hmdHeight = frames.maxHmdHeight + if (hmdHeight <= 0.50f) { + LogManager + .warning( + "[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): $hmdHeight" + ) + } else { + LogManager.info("[AutoBone] Max headset height detected: $hmdHeight") + } + + // Estimate target height from HMD height + targetHeight = hmdHeight + } + return targetHeight + } + + @Throws(AutoBoneException::class) + fun processFrames( + frames: PoseFrames, + calcInitError: Boolean = true, + targetHeight: Float = -1f, + epochCallback: Consumer?, + ): AutoBoneResults { + var targetHeight = targetHeight + val frameCount = frames.maxFrameCount + val frames1 = PoseFrames(frames) + val frames2 = PoseFrames(frames) + val trackers1 = frames1.trackers + val trackers2 = frames2.trackers + + // Reload configs and detect chest tracker from the first frame + reloadConfigValues(trackers1) + val skeleton1 = HumanPoseManager( + trackers1 + ) + val skeleton2 = HumanPoseManager( + trackers2 + ) + val intermediateOffsets = EnumMap( + offsets + ) + val trainingStep = AutoBoneTrainingStep( + targetHeight, + skeleton1, + skeleton2, + frames, + intermediateOffsets + ) + skeleton1.setLegTweaksEnabled(false) + skeleton2.setLegTweaksEnabled(false) + + // If target height isn't specified, auto-detect + if (targetHeight < 0f) { + targetHeight = getTargetHeight(frames) + } + val errorStats = StatsCalculator() + + // Epoch loop, each epoch is one full iteration over the full dataset + for (epoch in (if (calcInitError) -1 else 0) until config.numEpochs) { + val adjustRate = if (epoch >= 0) { + ( + config.initialAdjustRate + * FastMath.pow(config.adjustRateMultiplier, epoch.toFloat()) + ) + } else { + 0f + } + var randomFrameIndices: IntArray? = null + if (config.randomizeFrameOrder) { + randomFrameIndices = IntArray(frameCount) + var zeroPos = -1 + for (i in 0 until frameCount) { + var index = rand.nextInt(frameCount) + if (i > 0) { + while (index == zeroPos || randomFrameIndices[index] > 0) { + index = rand.nextInt(frameCount) + } + } else { + zeroPos = index + } + randomFrameIndices[index] = i + } + } + + // Iterate over the frames using a cursor and an offset for comparing + // frames a certain number of frames apart + var cursorOffset = config.minDataDistance + while (cursorOffset <= config.maxDataDistance && + cursorOffset < frameCount + ) { + var frameCursor = 0 + while (frameCursor < frameCount - cursorOffset) { + val frameCursor2 = frameCursor + cursorOffset + applyConfig(skeleton1) + applyConfig(skeleton2) + if (config.randomizeFrameOrder) { + trainingStep + .setCursors( + randomFrameIndices!![frameCursor], + randomFrameIndices[frameCursor2] + ) + } else { + trainingStep.setCursors(frameCursor, frameCursor2) + } + frames1.setCursors(trainingStep.cursor1) + frames2.setCursors(trainingStep.cursor2) + skeleton1.update() + skeleton2.update() + val totalLength = getLengthSum(offsets) + val curHeight = sumSelectConfigs(heightOffsets, offsets) + trainingStep.currentHeight = curHeight + val errorDeriv = getErrorDeriv(trainingStep) + val error = errorFunc(errorDeriv) + + // In case of fire + if (java.lang.Float.isNaN(error) || java.lang.Float.isInfinite(error)) { + // Extinguish + LogManager + .warning( + "[AutoBone] Error value is invalid, resetting variables to recover" + ) + reloadConfigValues(trackers1) + + // Reset error sum values + errorStats.reset() + frameCursor += config.cursorIncrement + + // Continue on new data + continue + } + + // Store the error count for logging purposes + errorStats.addValue(errorDeriv) + val adjustVal = error * adjustRate + + // If there is no adjustment whatsoever, skip this + if (adjustVal == 0f) { + frameCursor += config.cursorIncrement + continue + } + val slideLeft = skeleton2 + .getComputedTracker(TrackerRole.LEFT_FOOT).position + .subtract( + skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position + ) + val slideRight = skeleton2 + .getComputedTracker(TrackerRole.RIGHT_FOOT).position + .subtract( + skeleton1 + .getComputedTracker(TrackerRole.RIGHT_FOOT).position + ) + intermediateOffsets.putAll(offsets) + for (entry in offsets.entries) { + // Skip adjustment if the epoch is before starting (for + // logging only) + if (epoch < 0) { + break + } + val originalLength = entry.value + val isHeightVar = heightOffsets.contains(entry.key) + val leftDotProduct = getDotProductDiff( + skeleton1, + skeleton2, + entry.key, + false, + slideLeft + ) + val rightDotProduct = getDotProductDiff( + skeleton1, + skeleton2, + entry.key, + true, + slideRight + ) + val dotLength = ( + originalLength + * ((leftDotProduct + rightDotProduct) / 2f) + ) + + // Scale by the ratio for smooth adjustment and more + // stable results + val curAdjustVal = adjustVal * -dotLength / totalLength + val newLength = originalLength + curAdjustVal + + // No small or negative numbers!!! Bad algorithm! + if (newLength < 0.01f) { + continue + } + + // Apply new offset length + intermediateOffsets[entry.key] = newLength + applyConfig(skeleton1, intermediateOffsets) + applyConfig(skeleton2, intermediateOffsets) + + // Update the skeleton poses for the new offset length + skeleton1.update() + skeleton2.update() + val newHeight = if (isHeightVar) curHeight + curAdjustVal else curHeight + trainingStep.currentHeight = newHeight + val newErrorDeriv = getErrorDeriv(trainingStep) + if (newErrorDeriv < errorDeriv) { + entry.setValue(newLength) + } + + // Reset the length to minimize bias in other variables, + // it's applied later + intermediateOffsets[entry.key] = originalLength + applyConfig(skeleton1, intermediateOffsets) + applyConfig(skeleton2, intermediateOffsets) + } + if (config.scaleEachStep) { + val stepHeight = sumSelectConfigs(heightOffsets, offsets) + if (stepHeight > 0f) { + val stepHeightDiff = targetHeight - stepHeight + for (entry in offsets.entries) { + // Only height variables + if (entry.key == BoneType.NECK || + !heightOffsets.contains(entry.key) + ) { + continue + } + val length = entry.value + + // Multiply the diff by the length to height + // ratio + val adjVal = stepHeightDiff * (length / stepHeight) + + // Scale the length to fit the target height + entry.setValue(Math.max(length + adjVal / 2f, 0.01f)) + } + } + } + frameCursor += config.cursorIncrement + } + cursorOffset++ + } + + // Calculate average error over the epoch + if (epoch <= 0 || epoch >= config.numEpochs - 1 || (epoch + 1) % config.printEveryNumEpochs == 0) { + LogManager + .info( + "[AutoBone] Epoch ${epoch + 1} average error: ${errorStats.mean} (SD ${errorStats.standardDeviation})" + ) + } + applyConfig(legacyConfigs) + epochCallback?.accept(Epoch(epoch + 1, config.numEpochs, errorStats, legacyConfigs)) + } + val finalHeight = sumSelectConfigs(heightOffsets, offsets) + LogManager + .info( + "[AutoBone] Target height: $targetHeight New height: $finalHeight" + ) + return AutoBoneResults(finalHeight, targetHeight, errorStats, legacyConfigs) + } + + @Throws(AutoBoneException::class) + protected fun getErrorDeriv(trainingStep: AutoBoneTrainingStep): Float { + var sumError = 0f + if (config.slideErrorFactor > 0f) { + sumError += slideError.getStepError(trainingStep) * config.slideErrorFactor + } + if (config.offsetSlideErrorFactor > 0f) { + sumError += ( + offsetSlideError.getStepError(trainingStep) * + config.offsetSlideErrorFactor + ) + } + if (config.footHeightOffsetErrorFactor > 0f) { + sumError += ( + footHeightOffsetError.getStepError(trainingStep) * + config.footHeightOffsetErrorFactor + ) + } + if (config.bodyProportionErrorFactor > 0f) { + sumError += ( + bodyProportionError.getStepError(trainingStep) * + config.bodyProportionErrorFactor + ) + } + if (config.heightErrorFactor > 0f) { + sumError += heightError.getStepError(trainingStep) * config.heightErrorFactor + } + if (config.positionErrorFactor > 0f) { + sumError += ( + positionError.getStepError(trainingStep) * + config.positionErrorFactor + ) + } + if (config.positionOffsetErrorFactor > 0f) { + sumError += ( + positionOffsetError.getStepError(trainingStep) * + config.positionOffsetErrorFactor + ) + } + return sumError + } + + val lengthsString: String + get() { + val configInfo = StringBuilder() + offsets.forEach { (key: BoneType, value: Float) -> + if (configInfo.length > 0) { + configInfo.append(", ") + } + configInfo + .append(key.toString()) + .append(": ") + .append(StringUtils.prettyNumber(value * 100f, 2)) + } + return configInfo.toString() + } + + fun saveRecording(frames: PoseFrames?, recordingFile: File) { + if (saveDir.isDirectory || saveDir.mkdirs()) { + LogManager + .info("[AutoBone] Exporting frames to \"${recordingFile.path}\"...") + if (PoseFrameIO.writeToFile(recordingFile, frames)) { + LogManager + .info( + "[AutoBone] Done exporting! Recording can be found at \"${recordingFile.path}\"." + ) + } else { + LogManager + .severe( + "[AutoBone] Failed to export the recording to \"${recordingFile.path}\"." + ) + } + } else { + LogManager + .severe( + "[AutoBone] Failed to create the recording directory \"${saveDir.path}\"." + ) + } + } + + fun saveRecording(frames: PoseFrames, recordingFileName: String) { + saveRecording(frames, File(saveDir, recordingFileName)) + } + + fun saveRecording(frames: PoseFrames) { + var recordingFile: File + var recordingIndex = 1 + do { + recordingFile = File(saveDir, "ABRecording${recordingIndex++}.pfr") + } while (recordingFile.exists()) + saveRecording(frames, recordingFile) + } + + fun loadRecordings(): FastList> { + val recordings = FastList>() + if (loadDir.isDirectory) { + val files = loadDir.listFiles() + if (files != null) { + for (file in files) { + if (file.isFile && + org.apache.commons.lang3.StringUtils + .endsWithIgnoreCase(file.name, ".pfr") + ) { + LogManager + .info( + "[AutoBone] Detected recording at \"${file.path}\", loading frames..." + ) + val frames = PoseFrameIO.readFromFile(file) + if (frames == null) { + LogManager + .severe("Reading frames from \"${file.path}\" failed...") + } else { + recordings.add(Pair.of(file.name, frames)) + } + } + } + } + } + return recordings + } + + inner class Epoch( + val epoch: Int, + val totalEpochs: Int, + val epochError: StatsCalculator, + val configValues: EnumMap, + ) { + override fun toString(): String { + return "Epoch: $epoch, Epoch Error: $epochError" + } + } + + inner class AutoBoneResults( + val finalHeight: Float, + val targetHeight: Float, + val epochError: StatsCalculator, + val configValues: EnumMap, + ) { + val heightDifference: Float + get() = FastMath.abs(targetHeight - finalHeight) + } + + companion object { + val saveDir = File("AutoBone Recordings") + val loadDir = File("Load AutoBone Recordings") + + // Mean square error function + protected fun errorFunc(errorDeriv: Float): Float { + return 0.5f * (errorDeriv * errorDeriv) + } + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java b/server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java deleted file mode 100644 index 2e4752457..000000000 --- a/server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java +++ /dev/null @@ -1,436 +0,0 @@ -package dev.slimevr.autobone; - -import dev.slimevr.VRServer; -import dev.slimevr.autobone.AutoBone.AutoBoneResults; -import dev.slimevr.autobone.errors.AutoBoneException; -import dev.slimevr.poserecorder.*; -import dev.slimevr.tracking.processor.config.SkeletonConfigManager; -import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets; -import io.eiren.util.StringUtils; -import io.eiren.util.logging.LogManager; -import org.apache.commons.lang3.tuple.Pair; - -import java.util.List; -import java.util.concurrent.CopyOnWriteArrayList; -import java.util.concurrent.Future; -import java.util.concurrent.locks.ReentrantLock; - - -public class AutoBoneHandler { - - private final VRServer server; - private final PoseRecorder poseRecorder; - private final AutoBone autoBone; - - private ReentrantLock recordingLock = new ReentrantLock(); - private Thread recordingThread = null; - - private ReentrantLock saveRecordingLock = new ReentrantLock(); - private Thread saveRecordingThread = null; - - private ReentrantLock autoBoneLock = new ReentrantLock(); - private Thread autoBoneThread = null; - - private List listeners = new CopyOnWriteArrayList<>(); - - public AutoBoneHandler(VRServer server) { - this.server = server; - this.poseRecorder = new PoseRecorder(server); - this.autoBone = new AutoBone(server); - } - - public void addListener(AutoBoneListener listener) { - this.listeners.add(listener); - } - - public void removeListener(AutoBoneListener listener) { - this.listeners.removeIf(l -> listener == l); - } - - private void announceProcessStatus( - AutoBoneProcessType processType, - String message, - long current, - long total, - boolean completed, - boolean success - ) { - listeners - .forEach( - listener -> listener - .onAutoBoneProcessStatus( - processType, - message, - current, - total, - completed, - success - ) - ); - } - - private void announceProcessStatus( - AutoBoneProcessType processType, - String message, - boolean completed, - boolean success - ) { - announceProcessStatus(processType, message, 0, 0, completed, success); - } - - private void announceProcessStatus(AutoBoneProcessType processType, String message) { - announceProcessStatus(processType, message, false, true); - } - - private void announceProcessStatus(AutoBoneProcessType processType, long current, long total) { - announceProcessStatus(processType, null, current, total, false, true); - } - - public String getLengthsString() { - return autoBone.getLengthsString(); - } - - private AutoBoneResults processFrames(PoseFrames frames) throws AutoBoneException { - return autoBone - .processFrames( - frames, - autoBone.getConfig().calcInitError, - autoBone.getConfig().targetHeight, - (epoch) -> listeners.forEach(listener -> listener.onAutoBoneEpoch(epoch)) - ); - } - - public boolean startProcessByType(AutoBoneProcessType processType) { - switch (processType) { - case RECORD: - startRecording(); - break; - - case SAVE: - saveRecording(); - break; - - case PROCESS: - processRecording(); - break; - - case APPLY: - applyValues(); - break; - - default: - return false; - } - - return true; - } - - public void startRecording() { - recordingLock.lock(); - - try { - // Prevent running multiple times - if (recordingThread != null) { - return; - } - - Thread thread = new Thread(this::startRecordingThread); - recordingThread = thread; - thread.start(); - } finally { - recordingLock.unlock(); - } - } - - private void startRecordingThread() { - try { - if (poseRecorder.isReadyToRecord()) { - announceProcessStatus(AutoBoneProcessType.RECORD, "Recording..."); - - // 1000 samples at 20 ms per sample is 20 seconds - int sampleCount = this.autoBone.getConfig().sampleCount; - long sampleRate = this.autoBone.getConfig().sampleRateMs; - Future framesFuture = poseRecorder - .startFrameRecording( - sampleCount, - sampleRate, - progress -> announceProcessStatus( - AutoBoneProcessType.RECORD, - progress.frame, - progress.totalFrames - ) - ); - PoseFrames frames = framesFuture.get(); - LogManager.info("[AutoBone] Done recording!"); - - // Save a recurring recording for users to send as debug info - announceProcessStatus(AutoBoneProcessType.RECORD, "Saving recording..."); - autoBone.saveRecording(frames, "LastABRecording.pfr"); - - if (this.autoBone.getConfig().saveRecordings) { - announceProcessStatus( - AutoBoneProcessType.RECORD, - "Saving recording (from config option)..." - ); - autoBone.saveRecording(frames); - } - - listeners.forEach(listener -> listener.onAutoBoneRecordingEnd(frames)); - - announceProcessStatus(AutoBoneProcessType.RECORD, "Done recording!", true, true); - } else { - announceProcessStatus( - AutoBoneProcessType.RECORD, - "The server is not ready to record", - true, - false - ); - LogManager.severe("[AutoBone] Unable to record..."); - return; - } - } catch (Exception e) { - announceProcessStatus( - AutoBoneProcessType.RECORD, - String.format("Recording failed: %s", e.getMessage()), - true, - false - ); - LogManager.severe("[AutoBone] Failed recording!", e); - } finally { - recordingThread = null; - } - } - - public void saveRecording() { - saveRecordingLock.lock(); - - try { - // Prevent running multiple times - if (saveRecordingThread != null) { - return; - } - - Thread thread = new Thread(this::saveRecordingThread); - saveRecordingThread = thread; - thread.start(); - } finally { - saveRecordingLock.unlock(); - } - } - - private void saveRecordingThread() { - try { - Future framesFuture = poseRecorder.getFramesAsync(); - if (framesFuture != null) { - announceProcessStatus(AutoBoneProcessType.SAVE, "Waiting for recording..."); - PoseFrames frames = framesFuture.get(); - - if (frames.getTrackerCount() <= 0) { - throw new IllegalStateException("Recording has no trackers"); - } - - if (frames.getMaxFrameCount() <= 0) { - throw new IllegalStateException("Recording has no frames"); - } - - announceProcessStatus(AutoBoneProcessType.SAVE, "Saving recording..."); - autoBone.saveRecording(frames); - - announceProcessStatus(AutoBoneProcessType.SAVE, "Recording saved!", true, true); - } else { - announceProcessStatus(AutoBoneProcessType.SAVE, "No recording found", true, false); - LogManager.severe("[AutoBone] Unable to save, no recording was done..."); - return; - } - } catch (Exception e) { - announceProcessStatus( - AutoBoneProcessType.SAVE, - String.format("Failed to save recording: %s", e.getMessage()), - true, - false - ); - LogManager.severe("[AutoBone] Failed to save recording!", e); - } finally { - saveRecordingThread = null; - } - } - - public void processRecording() { - autoBoneLock.lock(); - - try { - // Prevent running multiple times - if (autoBoneThread != null) { - return; - } - - Thread thread = new Thread(this::processRecordingThread); - autoBoneThread = thread; - thread.start(); - } finally { - autoBoneLock.unlock(); - } - } - - private void processRecordingThread() { - try { - announceProcessStatus(AutoBoneProcessType.PROCESS, "Loading recordings..."); - List> frameRecordings = autoBone.loadRecordings(); - - if (!frameRecordings.isEmpty()) { - LogManager.info("[AutoBone] Done loading frames!"); - } else { - Future framesFuture = poseRecorder.getFramesAsync(); - if (framesFuture != null) { - announceProcessStatus(AutoBoneProcessType.PROCESS, "Waiting for recording..."); - PoseFrames frames = framesFuture.get(); - - if (frames.getTrackerCount() <= 0) { - throw new IllegalStateException("Recording has no trackers"); - } - - if (frames.getMaxFrameCount() <= 0) { - throw new IllegalStateException("Recording has no frames"); - } - - frameRecordings.add(Pair.of("", frames)); - } else { - announceProcessStatus( - AutoBoneProcessType.PROCESS, - "No recordings found...", - true, - false - ); - LogManager - .severe( - "[AutoBone] No recordings found in \"" - + AutoBone.getLoadDir().getPath() - + "\" and no recording was done..." - ); - return; - } - } - - announceProcessStatus(AutoBoneProcessType.PROCESS, "Processing recording(s)..."); - LogManager.info("[AutoBone] Processing frames..."); - StatsCalculator errorStats = new StatsCalculator(); - SkeletonConfigManager skeletonConfigManagerBuffer = new SkeletonConfigManager(false); - for (Pair recording : frameRecordings) { - LogManager - .info("[AutoBone] Processing frames from \"" + recording.getKey() + "\"..."); - - List trackers = recording.getValue().getTrackers(); - - StringBuilder trackerInfo = new StringBuilder(); - for (PoseFrameTracker tracker : trackers) { - if (tracker == null) - continue; - - TrackerFrame frame = tracker.safeGetFrame(0); - if (frame == null || !frame.hasData(TrackerFrameData.DESIGNATION)) - continue; - - if (trackerInfo.length() > 0) { - trackerInfo.append(", "); - } - - trackerInfo.append(frame.designation.designation); - - if (frame.hasData(TrackerFrameData.POSITION)) { - trackerInfo.append(" (P)"); - } - } - - LogManager - .info( - "[AutoBone] (" - + trackers.size() - + " trackers) [" - + trackerInfo - + "]" - ); - - AutoBoneResults autoBoneResults = processFrames(recording.getValue()); - errorStats.addValue(autoBoneResults.getHeightDifference()); - LogManager.info("[AutoBone] Done processing!"); - - // #region Stats/Values - skeletonConfigManagerBuffer.setOffsets(autoBoneResults.configValues); - - float neckLength = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.NECK); - float chestLength = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.CHEST); - float waistLength = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.WAIST); - float hipLength = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.HIP); - float torsoLength = chestLength + waistLength + hipLength; - float hipWidth = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.HIPS_WIDTH); - float legLength = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.UPPER_LEG) - + skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.LOWER_LEG); - float lowerLegLength = skeletonConfigManagerBuffer - .getOffset(SkeletonConfigOffsets.LOWER_LEG); - - float neckTorso = neckLength / torsoLength; - float chestTorso = chestLength / torsoLength; - float torsoWaist = hipWidth / torsoLength; - float legTorso = legLength / torsoLength; - float legBody = legLength / (torsoLength + neckLength); - float kneeLeg = lowerLegLength / legLength; - - LogManager - .info( - "[AutoBone] Ratios: [{Neck-Torso: " - + StringUtils.prettyNumber(neckTorso) - + "}, {Chest-Torso: " - + StringUtils.prettyNumber(chestTorso) - + "}, {Torso-Waist: " - + StringUtils.prettyNumber(torsoWaist) - + "}, {Leg-Torso: " - + StringUtils.prettyNumber(legTorso) - + "}, {Leg-Body: " - + StringUtils.prettyNumber(legBody) - + "}, {Knee-Leg: " - + StringUtils.prettyNumber(kneeLeg) - + "}]" - ); - LogManager.info("[AutoBone] Length values: " + autoBone.getLengthsString()); - } - - LogManager - .info( - "[AutoBone] Average height error: " - + StringUtils.prettyNumber(errorStats.getMean(), 6) - + " (SD " - + StringUtils.prettyNumber(errorStats.getStandardDeviation(), 6) - + ")" - ); - // #endregion - - listeners.forEach(listener -> listener.onAutoBoneEnd(autoBone.legacyConfigs)); - - announceProcessStatus(AutoBoneProcessType.PROCESS, "Done processing!", true, true); - } catch (Exception e) { - announceProcessStatus( - AutoBoneProcessType.PROCESS, - String.format("Processing failed: %s", e.getMessage()), - true, - false - ); - LogManager.severe("[AutoBone] Failed adjustment!", e); - } finally { - autoBoneThread = null; - } - } - - public void applyValues() { - autoBone.applyAndSaveConfig(); - announceProcessStatus(AutoBoneProcessType.APPLY, "Adjusted values applied!", true, true); - // TODO Update GUI values after applying? Is that needed here? - } - -} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.kt b/server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.kt new file mode 100644 index 000000000..d51306203 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/AutoBoneHandler.kt @@ -0,0 +1,405 @@ +package dev.slimevr.autobone + +import dev.slimevr.VRServer +import dev.slimevr.autobone.AutoBone.AutoBoneResults +import dev.slimevr.autobone.AutoBone.Companion.loadDir +import dev.slimevr.autobone.AutoBone.Epoch +import dev.slimevr.autobone.errors.AutoBoneException +import dev.slimevr.poserecorder.PoseFrames +import dev.slimevr.poserecorder.PoseRecorder +import dev.slimevr.poserecorder.PoseRecorder.RecordingProgress +import dev.slimevr.poserecorder.TrackerFrameData +import dev.slimevr.tracking.processor.config.SkeletonConfigManager +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import io.eiren.util.StringUtils +import io.eiren.util.logging.LogManager +import org.apache.commons.lang3.tuple.Pair +import java.util.concurrent.CopyOnWriteArrayList +import java.util.concurrent.locks.ReentrantLock +import java.util.function.Consumer +import kotlin.concurrent.thread +import kotlin.concurrent.withLock + +class AutoBoneHandler(private val server: VRServer) { + private val poseRecorder: PoseRecorder = PoseRecorder(server) + private val autoBone: AutoBone = AutoBone(server) + + private val recordingLock = ReentrantLock() + private var recordingThread: Thread? = null + private val saveRecordingLock = ReentrantLock() + private var saveRecordingThread: Thread? = null + private val autoBoneLock = ReentrantLock() + private var autoBoneThread: Thread? = null + + private val listeners = CopyOnWriteArrayList() + + fun addListener(listener: AutoBoneListener) { + listeners.add(listener) + } + + fun removeListener(listener: AutoBoneListener) { + listeners.removeIf { l: AutoBoneListener -> listener === l } + } + + private fun announceProcessStatus( + processType: AutoBoneProcessType, + message: String?, + current: Long, + total: Long, + completed: Boolean, + success: Boolean, + ) { + listeners + .forEach( + Consumer { listener: AutoBoneListener -> + listener + .onAutoBoneProcessStatus( + processType, + message, + current, + total, + completed, + success + ) + } + ) + } + + private fun announceProcessStatus( + processType: AutoBoneProcessType, + message: String, + completed: Boolean = false, + success: Boolean = true, + ) { + announceProcessStatus(processType, message, 0, 0, completed, success) + } + + private fun announceProcessStatus(processType: AutoBoneProcessType, current: Long, total: Long) { + announceProcessStatus(processType, null, current, total, completed = false, success = true) + } + + val lengthsString: String + get() = autoBone.lengthsString + + @Throws(AutoBoneException::class) + private fun processFrames(frames: PoseFrames): AutoBoneResults { + return autoBone + .processFrames( + frames, + autoBone.config.calcInitError, + autoBone.config.targetHeight + ) { epoch: Epoch? -> listeners.forEach(Consumer { listener: AutoBoneListener -> listener.onAutoBoneEpoch(epoch!!) }) } + } + + fun startProcessByType(processType: AutoBoneProcessType): Boolean { + when (processType) { + AutoBoneProcessType.RECORD -> startRecording() + AutoBoneProcessType.SAVE -> saveRecording() + AutoBoneProcessType.PROCESS -> processRecording() + AutoBoneProcessType.APPLY -> applyValues() + else -> { + return false + } + } + return true + } + + fun startRecording() { + recordingLock.withLock { + // Prevent running multiple times + if (recordingThread != null) { + return + } + recordingThread = thread(start = true) { startRecordingThread() } + } + } + + private fun startRecordingThread() { + try { + if (poseRecorder.isReadyToRecord) { + announceProcessStatus(AutoBoneProcessType.RECORD, "Recording...") + + // 1000 samples at 20 ms per sample is 20 seconds + val sampleCount = autoBone.config.sampleCount + val sampleRate = autoBone.config.sampleRateMs + val framesFuture = poseRecorder + .startFrameRecording( + sampleCount, + sampleRate + ) { progress: RecordingProgress -> + announceProcessStatus( + AutoBoneProcessType.RECORD, + progress.frame.toLong(), + progress.totalFrames + .toLong() + ) + } + val frames = framesFuture.get() + LogManager.info("[AutoBone] Done recording!") + + // Save a recurring recording for users to send as debug info + announceProcessStatus(AutoBoneProcessType.RECORD, "Saving recording...") + autoBone.saveRecording(frames, "LastABRecording.pfr") + if (autoBone.config.saveRecordings) { + announceProcessStatus( + AutoBoneProcessType.RECORD, + "Saving recording (from config option)..." + ) + autoBone.saveRecording(frames) + } + listeners.forEach(Consumer { listener: AutoBoneListener -> listener.onAutoBoneRecordingEnd(frames) }) + announceProcessStatus( + AutoBoneProcessType.RECORD, + "Done recording!", + completed = true, + success = true + ) + } else { + announceProcessStatus( + AutoBoneProcessType.RECORD, + "The server is not ready to record", + completed = true, + success = false + ) + LogManager.severe("[AutoBone] Unable to record...") + return + } + } catch (e: Exception) { + announceProcessStatus( + AutoBoneProcessType.RECORD, + String.format("Recording failed: %s", e.message), + completed = true, + success = false + ) + LogManager.severe("[AutoBone] Failed recording!", e) + } finally { + recordingThread = null + } + } + + fun saveRecording() { + saveRecordingLock.withLock { + // Prevent running multiple times + if (saveRecordingThread != null) { + return + } + saveRecordingThread = thread(start = true) { saveRecordingThread() } + } + } + + private fun saveRecordingThread() { + try { + val framesFuture = poseRecorder.framesAsync + if (framesFuture != null) { + announceProcessStatus(AutoBoneProcessType.SAVE, "Waiting for recording...") + val frames = framesFuture.get() + check(frames.trackerCount > 0) { "Recording has no trackers" } + check(frames.maxFrameCount > 0) { "Recording has no frames" } + announceProcessStatus(AutoBoneProcessType.SAVE, "Saving recording...") + autoBone.saveRecording(frames) + announceProcessStatus( + AutoBoneProcessType.SAVE, + "Recording saved!", + completed = true, + success = true + ) + } else { + announceProcessStatus( + AutoBoneProcessType.SAVE, + "No recording found", + completed = true, + success = false + ) + LogManager.severe("[AutoBone] Unable to save, no recording was done...") + return + } + } catch (e: Exception) { + announceProcessStatus( + AutoBoneProcessType.SAVE, + String.format("Failed to save recording: %s", e.message), + completed = true, + success = false + ) + LogManager.severe("[AutoBone] Failed to save recording!", e) + } finally { + saveRecordingThread = null + } + } + + fun processRecording() { + autoBoneLock.withLock { + // Prevent running multiple times + if (autoBoneThread != null) { + return + } + autoBoneThread = thread(start = true) { processRecordingThread() } + } + } + + private fun processRecordingThread() { + try { + announceProcessStatus(AutoBoneProcessType.PROCESS, "Loading recordings...") + val frameRecordings = autoBone.loadRecordings() + if (!frameRecordings.isEmpty()) { + LogManager.info("[AutoBone] Done loading frames!") + } else { + val framesFuture = poseRecorder.framesAsync + if (framesFuture != null) { + announceProcessStatus(AutoBoneProcessType.PROCESS, "Waiting for recording...") + val frames = framesFuture.get() + check(frames.trackerCount > 0) { "Recording has no trackers" } + check(frames.maxFrameCount > 0) { "Recording has no frames" } + frameRecordings.add(Pair.of("", frames)) + } else { + announceProcessStatus( + AutoBoneProcessType.PROCESS, + "No recordings found...", + completed = true, + success = false + ) + LogManager + .severe( + "[AutoBone] No recordings found in \"" + + loadDir.path + + "\" and no recording was done..." + ) + return + } + } + announceProcessStatus(AutoBoneProcessType.PROCESS, "Processing recording(s)...") + LogManager.info("[AutoBone] Processing frames...") + val errorStats = StatsCalculator() + val skeletonConfigManagerBuffer = SkeletonConfigManager(false) + for ((key, value) in frameRecordings) { + LogManager + .info("[AutoBone] Processing frames from \"$key\"...") + val trackers = value.trackers + val trackerInfo = StringBuilder() + for (tracker in trackers) { + if (tracker == null) continue + val frame = tracker.safeGetFrame(0) + if (frame == null || frame.bodyPosition == null) continue + + // Add a comma if this is not the first item listed + if (trackerInfo.isNotEmpty()) { + trackerInfo.append(", ") + } + trackerInfo.append(frame.bodyPosition.designation) + + // Represent the data flags + val trackerFlags = StringBuilder() + if (frame.hasData(TrackerFrameData.ROTATION)) { + trackerFlags.append("R") + } + if (frame.hasData(TrackerFrameData.POSITION)) { + trackerFlags.append("P") + } + if (frame.hasData(TrackerFrameData.ACCELERATION)) { + trackerFlags.append("A") + } + if (frame.hasData(TrackerFrameData.RAW_ROTATION)) { + trackerFlags.append("r") + } + + // If there are data flags, print them in brackets after the + // designation + if (trackerFlags.isNotEmpty()) { + trackerInfo.append(" (").append(trackerFlags).append(")") + } + } + LogManager + .info( + "[AutoBone] (" + + trackers.size + + " trackers) [" + + trackerInfo + + "]" + ) + val autoBoneResults = processFrames(value) + errorStats.addValue(autoBoneResults.heightDifference) + LogManager.info("[AutoBone] Done processing!") + + // #region Stats/Values + skeletonConfigManagerBuffer.setOffsets(autoBoneResults.configValues) + val neckLength = skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.NECK) + val chestLength = skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.CHEST) + val waistLength = skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.WAIST) + val hipLength = skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.HIP) + val torsoLength = chestLength + waistLength + hipLength + val hipWidth = skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.HIPS_WIDTH) + val legLength = ( + skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.UPPER_LEG) + + skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.LOWER_LEG) + ) + val lowerLegLength = skeletonConfigManagerBuffer + .getOffset(SkeletonConfigOffsets.LOWER_LEG) + val neckTorso = neckLength / torsoLength + val chestTorso = chestLength / torsoLength + val torsoWaist = hipWidth / torsoLength + val legTorso = legLength / torsoLength + val legBody = legLength / (torsoLength + neckLength) + val kneeLeg = lowerLegLength / legLength + LogManager + .info( + "[AutoBone] Ratios: [{Neck-Torso: " + + StringUtils.prettyNumber(neckTorso) + + "}, {Chest-Torso: " + + StringUtils.prettyNumber(chestTorso) + + "}, {Torso-Waist: " + + StringUtils.prettyNumber(torsoWaist) + + "}, {Leg-Torso: " + + StringUtils.prettyNumber(legTorso) + + "}, {Leg-Body: " + + StringUtils.prettyNumber(legBody) + + "}, {Knee-Leg: " + + StringUtils.prettyNumber(kneeLeg) + + "}]" + ) + LogManager.info("[AutoBone] Length values: " + autoBone.lengthsString) + } + LogManager + .info( + "[AutoBone] Average height error: " + + StringUtils.prettyNumber(errorStats.mean, 6) + + " (SD " + + StringUtils.prettyNumber(errorStats.standardDeviation, 6) + + ")" + ) + // #endregion + listeners.forEach(Consumer { listener: AutoBoneListener -> listener.onAutoBoneEnd(autoBone.legacyConfigs) }) + announceProcessStatus( + AutoBoneProcessType.PROCESS, + "Done processing!", + completed = true, + success = true + ) + } catch (e: Exception) { + announceProcessStatus( + AutoBoneProcessType.PROCESS, + String.format("Processing failed: %s", e.message), + completed = true, + success = false + ) + LogManager.severe("[AutoBone] Failed adjustment!", e) + } finally { + autoBoneThread = null + } + } + + fun applyValues() { + autoBone.applyAndSaveConfig() + announceProcessStatus( + AutoBoneProcessType.APPLY, + "Adjusted values applied!", + completed = true, + success = true + ) + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneListener.java b/server/src/main/java/dev/slimevr/autobone/AutoBoneListener.java deleted file mode 100644 index 8c81dc3e2..000000000 --- a/server/src/main/java/dev/slimevr/autobone/AutoBoneListener.java +++ /dev/null @@ -1,26 +0,0 @@ -package dev.slimevr.autobone; - -import dev.slimevr.autobone.AutoBone.Epoch; -import dev.slimevr.poserecorder.PoseFrames; -import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets; - -import java.util.EnumMap; - - -public interface AutoBoneListener { - - void onAutoBoneProcessStatus( - AutoBoneProcessType processType, - String message, - long current, - long total, - boolean completed, - boolean success - ); - - void onAutoBoneRecordingEnd(PoseFrames recording); - - void onAutoBoneEpoch(Epoch epoch); - - void onAutoBoneEnd(EnumMap configValues); -} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneListener.kt b/server/src/main/java/dev/slimevr/autobone/AutoBoneListener.kt new file mode 100644 index 000000000..b78973071 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/AutoBoneListener.kt @@ -0,0 +1,21 @@ +package dev.slimevr.autobone + +import dev.slimevr.autobone.AutoBone.Epoch +import dev.slimevr.poserecorder.PoseFrames +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import java.util.EnumMap + +interface AutoBoneListener { + fun onAutoBoneProcessStatus( + processType: AutoBoneProcessType, + message: String?, + current: Long, + total: Long, + completed: Boolean, + success: Boolean, + ) + + fun onAutoBoneRecordingEnd(recording: PoseFrames) + fun onAutoBoneEpoch(epoch: Epoch) + fun onAutoBoneEnd(configValues: EnumMap) +} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.java b/server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.java deleted file mode 100644 index d2c88ea8b..000000000 --- a/server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.java +++ /dev/null @@ -1,31 +0,0 @@ -package dev.slimevr.autobone; - -import java.util.HashMap; -import java.util.Map; - - -public enum AutoBoneProcessType { - NONE(0), - RECORD(1), - SAVE(2), - PROCESS(3), - APPLY(4); - - public final int id; - - private static final Map byId = new HashMap<>(); - - AutoBoneProcessType(int id) { - this.id = id; - } - - public static AutoBoneProcessType getById(int id) { - return byId.get(id); - } - - static { - for (AutoBoneProcessType abpt : values()) { - byId.put(abpt.id, abpt); - } - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.kt b/server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.kt new file mode 100644 index 000000000..7736a2429 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/AutoBoneProcessType.kt @@ -0,0 +1,11 @@ +package dev.slimevr.autobone + +enum class AutoBoneProcessType(val id: Int) { + NONE(0), RECORD(1), SAVE(2), PROCESS(3), APPLY(4); + + companion object { + fun getById(id: Int): AutoBoneProcessType? = byId[id] + } +} + +private val byId = AutoBoneProcessType.values().associateBy { it.id } diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java b/server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java deleted file mode 100644 index 688baaad2..000000000 --- a/server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java +++ /dev/null @@ -1,108 +0,0 @@ -package dev.slimevr.autobone; - -import dev.slimevr.poserecorder.PoseFrames; -import dev.slimevr.tracking.processor.BoneType; -import dev.slimevr.tracking.processor.HumanPoseManager; - -import java.util.Map; - - -public class AutoBoneTrainingStep { - private int cursor1 = 0; - private int cursor2 = 0; - - private float currentHeight; - private final float targetHeight; - - private final HumanPoseManager humanPoseManager1; - private final HumanPoseManager humanPoseManager2; - - private final PoseFrames trainingFrames; - - private final Map intermediateOffsets; - - public AutoBoneTrainingStep( - int cursor1, - int cursor2, - float targetHeight, - HumanPoseManager humanPoseManager1, - HumanPoseManager humanPoseManager2, - PoseFrames trainingFrames, - Map intermediateOffsets - ) { - this.cursor1 = cursor1; - this.cursor2 = cursor2; - this.targetHeight = targetHeight; - this.humanPoseManager1 = humanPoseManager1; - this.humanPoseManager2 = humanPoseManager2; - this.trainingFrames = trainingFrames; - this.intermediateOffsets = intermediateOffsets; - } - - public AutoBoneTrainingStep( - float targetHeight, - HumanPoseManager humanPoseManager1, - HumanPoseManager humanPoseManager2, - PoseFrames trainingFrames, - Map intermediateOffsets - ) { - this.targetHeight = targetHeight; - this.humanPoseManager1 = humanPoseManager1; - this.humanPoseManager2 = humanPoseManager2; - this.trainingFrames = trainingFrames; - this.intermediateOffsets = intermediateOffsets; - } - - public int getCursor1() { - return cursor1; - } - - public void setCursor1(int cursor1) { - this.cursor1 = cursor1; - } - - public int getCursor2() { - return cursor2; - } - - public void setCursor2(int cursor2) { - this.cursor2 = cursor2; - } - - public void setCursors(int cursor1, int cursor2) { - this.cursor1 = cursor1; - this.cursor2 = cursor2; - } - - public float getCurrentHeight() { - return currentHeight; - } - - public void setCurrentHeight(float currentHeight) { - this.currentHeight = currentHeight; - } - - public float getTargetHeight() { - return targetHeight; - } - - public HumanPoseManager getHumanPoseManager1() { - return humanPoseManager1; - } - - public HumanPoseManager getHumanPoseManager2() { - return humanPoseManager2; - } - - public PoseFrames getTrainingFrames() { - return trainingFrames; - } - - public Map getIntermediateOffsets() { - return intermediateOffsets; - } - - public float getHeightOffset() { - return getTargetHeight() - getCurrentHeight(); - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.kt b/server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.kt new file mode 100644 index 000000000..e9ff05674 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.kt @@ -0,0 +1,25 @@ +package dev.slimevr.autobone + +import dev.slimevr.poserecorder.PoseFrames +import dev.slimevr.tracking.processor.BoneType +import dev.slimevr.tracking.processor.HumanPoseManager + +class AutoBoneTrainingStep( + val targetHeight: Float, + val humanPoseManager1: HumanPoseManager, + val humanPoseManager2: HumanPoseManager, + val trainingFrames: PoseFrames, + val intermediateOffsets: Map, + var cursor1: Int = 0, + var cursor2: Int = 0, + var currentHeight: Float = 0f, +) { + + fun setCursors(cursor1: Int, cursor2: Int) { + this.cursor1 = cursor1 + this.cursor2 = cursor2 + } + + val heightOffset: Float + get() = targetHeight - currentHeight +} diff --git a/server/src/main/java/dev/slimevr/autobone/StatsCalculator.java b/server/src/main/java/dev/slimevr/autobone/StatsCalculator.java deleted file mode 100644 index b40fa9bdc..000000000 --- a/server/src/main/java/dev/slimevr/autobone/StatsCalculator.java +++ /dev/null @@ -1,51 +0,0 @@ -package dev.slimevr.autobone; - -import com.jme3.math.FastMath; - - -/// This is a stat calculator based on Welford's online algorithm -/// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_online_algorithm -public class StatsCalculator { - - private int count = 0; - private float mean = 0f; - private float M2 = 0f; - - public void reset() { - count = 0; - mean = 0f; - M2 = 0f; - } - - public void addValue(float newValue) { - count += 1; - float delta = newValue - mean; - mean += delta / count; - float delta2 = newValue - mean; - M2 += delta * delta2; - } - - public float getMean() { - return mean; - } - - public float getVariance() { - if (count < 1) { - return Float.NaN; - } - - return M2 / count; - } - - public float getSampleVariance() { - if (count < 2) { - return Float.NaN; - } - - return M2 / (count - 1); - } - - public float getStandardDeviation() { - return FastMath.sqrt(getVariance()); - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/StatsCalculator.kt b/server/src/main/java/dev/slimevr/autobone/StatsCalculator.kt new file mode 100644 index 000000000..0d600b27b --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/StatsCalculator.kt @@ -0,0 +1,35 @@ +package dev.slimevr.autobone + +import com.jme3.math.FastMath + +/** + * This is a stat calculator based on Welford's online algorithm + * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford%27s_online_algorithm + */ +class StatsCalculator { + private var count = 0 + var mean = 0f + private set + private var m2 = 0f + + fun reset() { + count = 0 + mean = 0f + m2 = 0f + } + + fun addValue(newValue: Float) { + count += 1 + val delta = newValue - mean + mean += delta / count + val delta2 = newValue - mean + m2 += delta * delta2 + } + + val variance: Float + get() = if (count < 1) { Float.NaN } else { m2 / count } + val sampleVariance: Float + get() = if (count < 2) { Float.NaN } else { m2 / (count - 1) } + val standardDeviation: Float + get() = FastMath.sqrt(variance) +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java b/server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java deleted file mode 100644 index 130dee79f..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java +++ /dev/null @@ -1,28 +0,0 @@ -package dev.slimevr.autobone.errors; - -public class AutoBoneException extends Exception { - - public AutoBoneException() { - } - - public AutoBoneException(String message) { - super(message); - } - - public AutoBoneException(Throwable cause) { - super(cause); - } - - public AutoBoneException(String message, Throwable cause) { - super(message, cause); - } - - public AutoBoneException( - String message, - Throwable cause, - boolean enableSuppression, - boolean writableStackTrace - ) { - super(message, cause, enableSuppression, writableStackTrace); - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.kt b/server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.kt new file mode 100644 index 000000000..39646e552 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.kt @@ -0,0 +1,14 @@ +package dev.slimevr.autobone.errors + +class AutoBoneException : Exception { + constructor() + constructor(message: String?) : super(message) + constructor(cause: Throwable?) : super(cause) + constructor(message: String?, cause: Throwable?) : super(message, cause) + constructor( + message: String?, + cause: Throwable?, + enableSuppression: Boolean, + writableStackTrace: Boolean, + ) : super(message, cause, enableSuppression, writableStackTrace) +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java b/server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java deleted file mode 100644 index 5758fd616..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java +++ /dev/null @@ -1,138 +0,0 @@ -package dev.slimevr.autobone.errors; - - -import com.jme3.math.FastMath; -import dev.slimevr.autobone.AutoBoneTrainingStep; -import dev.slimevr.autobone.errors.proportions.ProportionLimiter; -import dev.slimevr.autobone.errors.proportions.RangeProportionLimiter; -import dev.slimevr.tracking.processor.HumanPoseManager; -import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets; - - -// The distance from average human proportions -public class BodyProportionError implements IAutoBoneError { - - // TODO hip tracker stuff... Hip tracker should be around 3 to 5 - // centimeters. - - // The headset height is not the full height! This value compensates for the - // offset from the headset height to the user height - public static float eyeHeightToHeightRatio = 0.936f; - - // Default config - // Height: 1.58 - // Full Height: 1.58 / 0.936 = 1.688034 - // Neck: 0.1 / 1.688034 = 0.059241 - // Torso: 0.56 / 1.688034 = 0.331747 - // Chest: 0.32 / 1.688034 = 0.18957 - // Waist: (0.56 - 0.32 - 0.04) / 1.688034 = 0.118481 - // Hip: 0.04 / 1.688034 = 0.023696 - // Hip Width: 0.26 / 1.688034 = 0.154025 - // Upper Leg: (0.92 - 0.50) / 1.688034 = 0.24881 - // Lower Leg: 0.50 / 1.688034 = 0.296203 - - // "Expected" are values from Drillis and Contini (1966) - // "Experimental" are values from experimentation by the SlimeVR community - public static final ProportionLimiter[] proportionLimits = new ProportionLimiter[] { - // Head - // Experimental: 0.059 - new RangeProportionLimiter( - 0.059f, - config -> config.getOffset(SkeletonConfigOffsets.HEAD), - 0.01f - ), - - // Neck - // Expected: 0.052 - // Experimental: 0.059 - new RangeProportionLimiter( - 0.054f, - config -> config.getOffset(SkeletonConfigOffsets.NECK), - 0.0015f - ), - - // Chest - // Experimental: 0.189 - new RangeProportionLimiter( - 0.189f, - config -> config.getOffset(SkeletonConfigOffsets.CHEST), - 0.02f - ), - - // Waist - // Experimental: 0.118 - new RangeProportionLimiter( - 0.118f, - config -> config.getOffset(SkeletonConfigOffsets.WAIST), - 0.05f - ), - - // Hip - // Experimental: 0.0237 - new RangeProportionLimiter( - 0.0237f, - config -> config.getOffset(SkeletonConfigOffsets.HIP), - 0.01f - ), - - // Hip Width - // Expected: 0.191 - // Experimental: 0.154 - new RangeProportionLimiter( - 0.184f, - config -> config.getOffset(SkeletonConfigOffsets.HIPS_WIDTH), - 0.04f - ), - - // Upper Leg - // Expected: 0.245 - new RangeProportionLimiter( - 0.245f, - config -> config.getOffset(SkeletonConfigOffsets.UPPER_LEG), - 0.015f - ), - - // Lower Leg - // Expected: 0.246 (0.285 including below ankle, could use a separate - // offset?) - new RangeProportionLimiter( - 0.285f, - config -> config.getOffset(SkeletonConfigOffsets.LOWER_LEG), - 0.02f - ), - }; - - public static ProportionLimiter getProportionLimitForOffset(SkeletonConfigOffsets offset) { - ProportionLimiter result = null; - switch (offset) { - case HEAD -> result = proportionLimits[0]; - case NECK -> result = proportionLimits[1]; - case CHEST -> result = proportionLimits[2]; - case WAIST -> result = proportionLimits[3]; - case HIP -> result = proportionLimits[4]; - case HIPS_WIDTH -> result = proportionLimits[5]; - case UPPER_LEG -> result = proportionLimits[6]; - case LOWER_LEG -> result = proportionLimits[7]; - } - return result; - } - - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - return getBodyProportionError( - trainingStep.getHumanPoseManager1(), - trainingStep.getCurrentHeight() - ); - } - - public float getBodyProportionError(HumanPoseManager humanPoseManager, float height) { - float fullHeight = height / eyeHeightToHeightRatio; - - float sum = 0f; - for (ProportionLimiter limiter : proportionLimits) { - sum += FastMath.abs(limiter.getProportionError(humanPoseManager, fullHeight)); - } - - return sum; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.kt b/server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.kt new file mode 100644 index 000000000..d705f1ce9 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.kt @@ -0,0 +1,127 @@ +package dev.slimevr.autobone.errors + +import com.jme3.math.FastMath +import dev.slimevr.autobone.AutoBoneTrainingStep +import dev.slimevr.autobone.errors.proportions.ProportionLimiter +import dev.slimevr.autobone.errors.proportions.RangeProportionLimiter +import dev.slimevr.tracking.processor.HumanPoseManager +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets + +// The distance from average human proportions +class BodyProportionError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + return getBodyProportionError( + trainingStep.humanPoseManager1, + trainingStep.currentHeight + ) + } + + fun getBodyProportionError(humanPoseManager: HumanPoseManager, height: Float): Float { + val fullHeight = height / eyeHeightToHeightRatio + var sum = 0f + for (limiter in proportionLimits) { + sum += FastMath.abs(limiter.getProportionError(humanPoseManager, fullHeight)) + } + return sum + } + + companion object { + // TODO hip tracker stuff... Hip tracker should be around 3 to 5 + // centimeters. + // The headset height is not the full height! This value compensates for the + // offset from the headset height to the user height + @JvmField + var eyeHeightToHeightRatio = 0.936f + + // Default config + // Height: 1.58 + // Full Height: 1.58 / 0.936 = 1.688034 + // Neck: 0.1 / 1.688034 = 0.059241 + // Torso: 0.56 / 1.688034 = 0.331747 + // Chest: 0.32 / 1.688034 = 0.18957 + // Waist: (0.56 - 0.32 - 0.04) / 1.688034 = 0.118481 + // Hip: 0.04 / 1.688034 = 0.023696 + // Hip Width: 0.26 / 1.688034 = 0.154025 + // Upper Leg: (0.92 - 0.50) / 1.688034 = 0.24881 + // Lower Leg: 0.50 / 1.688034 = 0.296203 + // "Expected" are values from Drillis and Contini (1966) + // "Experimental" are values from experimentation by the SlimeVR community + val proportionLimits = arrayOf( + // Head + // Experimental: 0.059 + RangeProportionLimiter( + 0.059f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.HEAD) }, + 0.01f + ), + // Neck + // Expected: 0.052 + // Experimental: 0.059 + RangeProportionLimiter( + 0.054f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.NECK) }, + 0.0015f + ), + // Chest + // Experimental: 0.189 + RangeProportionLimiter( + 0.189f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.CHEST) }, + 0.02f + ), + // Waist + // Experimental: 0.118 + RangeProportionLimiter( + 0.118f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.WAIST) }, + 0.05f + ), + // Hip + // Experimental: 0.0237 + RangeProportionLimiter( + 0.0237f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.HIP) }, + 0.01f + ), + // Hip Width + // Expected: 0.191 + // Experimental: 0.154 + RangeProportionLimiter( + 0.184f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.HIPS_WIDTH) }, + 0.04f + ), + // Upper Leg + // Expected: 0.245 + RangeProportionLimiter( + 0.245f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.UPPER_LEG) }, + 0.015f + ), + // Lower Leg + // Expected: 0.246 (0.285 including below ankle, could use a separate + // offset?) + RangeProportionLimiter( + 0.285f, + { config: HumanPoseManager -> config.getOffset(SkeletonConfigOffsets.LOWER_LEG) }, + 0.02f + ) + ) + + @JvmStatic + fun getProportionLimitForOffset(offset: SkeletonConfigOffsets): ProportionLimiter? { + return when (offset) { + SkeletonConfigOffsets.HEAD -> proportionLimits[0] + SkeletonConfigOffsets.NECK -> proportionLimits[1] + SkeletonConfigOffsets.CHEST -> proportionLimits[2] + SkeletonConfigOffsets.WAIST -> proportionLimits[3] + SkeletonConfigOffsets.HIP -> proportionLimits[4] + SkeletonConfigOffsets.HIPS_WIDTH -> proportionLimits[5] + SkeletonConfigOffsets.UPPER_LEG -> proportionLimits[6] + SkeletonConfigOffsets.LOWER_LEG -> proportionLimits[7] + else -> null + } + } + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java b/server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java deleted file mode 100644 index b5201e476..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java +++ /dev/null @@ -1,57 +0,0 @@ -package dev.slimevr.autobone.errors; - -import com.jme3.math.FastMath; - -import dev.slimevr.autobone.AutoBoneTrainingStep; -import dev.slimevr.tracking.processor.skeleton.HumanSkeleton; -import dev.slimevr.tracking.trackers.ComputedTracker; -import dev.slimevr.tracking.trackers.TrackerRole; - - -// The offset between the height both feet at one instant and over time -public class FootHeightOffsetError implements IAutoBoneError { - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - return getSlideError( - trainingStep.getHumanPoseManager1().getSkeleton(), - trainingStep.getHumanPoseManager2().getSkeleton() - ); - } - - public static float getSlideError(HumanSkeleton skeleton1, HumanSkeleton skeleton2) { - ComputedTracker leftTracker1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT); - ComputedTracker rightTracker1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT); - - ComputedTracker leftTracker2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT); - ComputedTracker rightTracker2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT); - - return getFootHeightError(leftTracker1, rightTracker1, leftTracker2, rightTracker2); - } - - public static float getFootHeightError( - ComputedTracker leftTracker1, - ComputedTracker rightTracker1, - ComputedTracker leftTracker2, - ComputedTracker rightTracker2 - ) { - float leftFoot1 = leftTracker1.position.y; - float rightFoot1 = rightTracker1.position.y; - - float leftFoot2 = leftTracker2.position.y; - float rightFoot2 = rightTracker2.position.y; - - // Compute all combinations of heights - float dist1 = FastMath.abs(leftFoot1 - rightFoot1); - float dist2 = FastMath.abs(leftFoot1 - leftFoot2); - float dist3 = FastMath.abs(leftFoot1 - rightFoot2); - - float dist4 = FastMath.abs(rightFoot1 - leftFoot2); - float dist5 = FastMath.abs(rightFoot1 - rightFoot2); - - float dist6 = FastMath.abs(leftFoot2 - rightFoot2); - - // Divide by 12 (6 values * 2 to halve) to halve and average, it's - // halved because you want to approach a midpoint, not the other point - return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.kt b/server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.kt new file mode 100644 index 000000000..d10bb2823 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.kt @@ -0,0 +1,52 @@ +package dev.slimevr.autobone.errors + +import com.jme3.math.FastMath +import dev.slimevr.autobone.AutoBoneTrainingStep +import dev.slimevr.tracking.processor.skeleton.HumanSkeleton +import dev.slimevr.tracking.trackers.ComputedTracker +import dev.slimevr.tracking.trackers.TrackerRole + +// The offset between the height both feet at one instant and over time +class FootHeightOffsetError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + return getSlideError( + trainingStep.humanPoseManager1.skeleton, + trainingStep.humanPoseManager2.skeleton + ) + } + + companion object { + fun getSlideError(skeleton1: HumanSkeleton, skeleton2: HumanSkeleton): Float { + val leftTracker1: ComputedTracker = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT) + val rightTracker1: ComputedTracker = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT) + val leftTracker2: ComputedTracker = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT) + val rightTracker2: ComputedTracker = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT) + return getFootHeightError(leftTracker1, rightTracker1, leftTracker2, rightTracker2) + } + + fun getFootHeightError( + leftTracker1: ComputedTracker, + rightTracker1: ComputedTracker, + leftTracker2: ComputedTracker, + rightTracker2: ComputedTracker, + ): Float { + val leftFoot1 = leftTracker1.position.y + val rightFoot1 = rightTracker1.position.y + val leftFoot2 = leftTracker2.position.y + val rightFoot2 = rightTracker2.position.y + + // Compute all combinations of heights + val dist1 = FastMath.abs(leftFoot1 - rightFoot1) + val dist2 = FastMath.abs(leftFoot1 - leftFoot2) + val dist3 = FastMath.abs(leftFoot1 - rightFoot2) + val dist4 = FastMath.abs(rightFoot1 - leftFoot2) + val dist5 = FastMath.abs(rightFoot1 - rightFoot2) + val dist6 = FastMath.abs(leftFoot2 - rightFoot2) + + // Divide by 12 (6 values * 2 to halve) to halve and average, it's + // halved because you want to approach a midpoint, not the other point + return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f + } + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/HeightError.java b/server/src/main/java/dev/slimevr/autobone/errors/HeightError.java deleted file mode 100644 index 1caae4df2..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/HeightError.java +++ /dev/null @@ -1,21 +0,0 @@ -package dev.slimevr.autobone.errors; - -import com.jme3.math.FastMath; - -import dev.slimevr.autobone.AutoBoneTrainingStep; - - -// The difference from the current height to the target height -public class HeightError implements IAutoBoneError { - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - return getHeightError( - trainingStep.getCurrentHeight(), - trainingStep.getTargetHeight() - ); - } - - public float getHeightError(float currentHeight, float targetHeight) { - return FastMath.abs(targetHeight - currentHeight); - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/HeightError.kt b/server/src/main/java/dev/slimevr/autobone/errors/HeightError.kt new file mode 100644 index 000000000..1c0b70a07 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/HeightError.kt @@ -0,0 +1,19 @@ +package dev.slimevr.autobone.errors + +import com.jme3.math.FastMath +import dev.slimevr.autobone.AutoBoneTrainingStep + +// The difference from the current height to the target height +class HeightError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + return getHeightError( + trainingStep.currentHeight, + trainingStep.targetHeight + ) + } + + fun getHeightError(currentHeight: Float, targetHeight: Float): Float { + return FastMath.abs(targetHeight - currentHeight) + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java b/server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java deleted file mode 100644 index 0ac0f4b52..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java +++ /dev/null @@ -1,8 +0,0 @@ -package dev.slimevr.autobone.errors; - -import dev.slimevr.autobone.AutoBoneTrainingStep; - - -public interface IAutoBoneError { - float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException; -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.kt b/server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.kt new file mode 100644 index 000000000..f8f54c617 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.kt @@ -0,0 +1,8 @@ +package dev.slimevr.autobone.errors + +import dev.slimevr.autobone.AutoBoneTrainingStep + +interface IAutoBoneError { + @Throws(AutoBoneException::class) + fun getStepError(trainingStep: AutoBoneTrainingStep): Float +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java b/server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java deleted file mode 100644 index dc6a7d418..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java +++ /dev/null @@ -1,64 +0,0 @@ -package dev.slimevr.autobone.errors; - -import com.jme3.math.FastMath; -import com.jme3.math.Vector3f; - -import dev.slimevr.autobone.AutoBoneTrainingStep; -import dev.slimevr.tracking.processor.skeleton.HumanSkeleton; -import dev.slimevr.tracking.trackers.ComputedTracker; -import dev.slimevr.tracking.trackers.TrackerRole; - - -// The change in distance between both of the ankles over time -public class OffsetSlideError implements IAutoBoneError { - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - return getSlideError( - trainingStep.getHumanPoseManager1().getSkeleton(), - trainingStep.getHumanPoseManager2().getSkeleton() - ); - } - - public static float getSlideError(HumanSkeleton skeleton1, HumanSkeleton skeleton2) { - ComputedTracker leftTracker1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT); - ComputedTracker rightTracker1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT); - - ComputedTracker leftTracker2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT); - ComputedTracker rightTracker2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT); - - return getSlideError(leftTracker1, rightTracker1, leftTracker2, rightTracker2); - } - - public static float getSlideError( - ComputedTracker leftTracker1, - ComputedTracker rightTracker1, - ComputedTracker leftTracker2, - ComputedTracker rightTracker2 - ) { - Vector3f leftFoot1 = leftTracker1.position; - Vector3f rightFoot1 = rightTracker1.position; - - Vector3f leftFoot2 = leftTracker2.position; - Vector3f rightFoot2 = rightTracker2.position; - - float slideDist1 = leftFoot1.distance(rightFoot1); - float slideDist2 = leftFoot2.distance(rightFoot2); - - float slideDist3 = leftFoot1.distance(rightFoot2); - float slideDist4 = leftFoot2.distance(rightFoot1); - - // Compute all combinations of distances - float dist1 = FastMath.abs(slideDist1 - slideDist2); - float dist2 = FastMath.abs(slideDist1 - slideDist3); - float dist3 = FastMath.abs(slideDist1 - slideDist4); - - float dist4 = FastMath.abs(slideDist2 - slideDist3); - float dist5 = FastMath.abs(slideDist2 - slideDist4); - - float dist6 = FastMath.abs(slideDist3 - slideDist4); - - // Divide by 12 (6 values * 2 to halve) to halve and average, it's - // halved because you want to approach a midpoint, not the other point - return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.kt b/server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.kt new file mode 100644 index 000000000..f88e12b8f --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.kt @@ -0,0 +1,56 @@ +package dev.slimevr.autobone.errors + +import com.jme3.math.FastMath +import dev.slimevr.autobone.AutoBoneTrainingStep +import dev.slimevr.tracking.processor.skeleton.HumanSkeleton +import dev.slimevr.tracking.trackers.ComputedTracker +import dev.slimevr.tracking.trackers.TrackerRole + +// The change in distance between both of the ankles over time +class OffsetSlideError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + return getSlideError( + trainingStep.humanPoseManager1.skeleton, + trainingStep.humanPoseManager2.skeleton + ) + } + + companion object { + fun getSlideError(skeleton1: HumanSkeleton, skeleton2: HumanSkeleton): Float { + val leftTracker1: ComputedTracker = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT) + val rightTracker1: ComputedTracker = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT) + val leftTracker2: ComputedTracker = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT) + val rightTracker2: ComputedTracker = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT) + return getSlideError(leftTracker1, rightTracker1, leftTracker2, rightTracker2) + } + + fun getSlideError( + leftTracker1: ComputedTracker, + rightTracker1: ComputedTracker, + leftTracker2: ComputedTracker, + rightTracker2: ComputedTracker, + ): Float { + val leftFoot1 = leftTracker1.position + val rightFoot1 = rightTracker1.position + val leftFoot2 = leftTracker2.position + val rightFoot2 = rightTracker2.position + val slideDist1 = leftFoot1.distance(rightFoot1) + val slideDist2 = leftFoot2.distance(rightFoot2) + val slideDist3 = leftFoot1.distance(rightFoot2) + val slideDist4 = leftFoot2.distance(rightFoot1) + + // Compute all combinations of distances + val dist1 = FastMath.abs(slideDist1 - slideDist2) + val dist2 = FastMath.abs(slideDist1 - slideDist3) + val dist3 = FastMath.abs(slideDist1 - slideDist4) + val dist4 = FastMath.abs(slideDist2 - slideDist3) + val dist5 = FastMath.abs(slideDist2 - slideDist4) + val dist6 = FastMath.abs(slideDist3 - slideDist4) + + // Divide by 12 (6 values * 2 to halve) to halve and average, it's + // halved because you want to approach a midpoint, not the other point + return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f + } + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/PositionError.java b/server/src/main/java/dev/slimevr/autobone/errors/PositionError.java deleted file mode 100644 index 608458158..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/PositionError.java +++ /dev/null @@ -1,62 +0,0 @@ -package dev.slimevr.autobone.errors; - -import java.util.List; - -import com.jme3.math.FastMath; - -import dev.slimevr.autobone.AutoBoneTrainingStep; -import dev.slimevr.poserecorder.PoseFrameTracker; -import dev.slimevr.poserecorder.TrackerFrame; -import dev.slimevr.poserecorder.TrackerFrameData; -import dev.slimevr.tracking.processor.skeleton.HumanSkeleton; -import dev.slimevr.tracking.trackers.ComputedTracker; - - -// The distance of any points to the corresponding absolute position -public class PositionError implements IAutoBoneError { - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - List trackers = trainingStep.getTrainingFrames().getTrackers(); - return (getPositionError( - trackers, - trainingStep.getCursor1(), - trainingStep.getHumanPoseManager1().getSkeleton() - ) - + getPositionError( - trackers, - trainingStep.getCursor2(), - trainingStep.getHumanPoseManager2().getSkeleton() - )) - / 2f; - } - - public static float getPositionError( - List trackers, - int cursor, - HumanSkeleton skeleton - ) { - float offset = 0f; - int offsetCount = 0; - - for (PoseFrameTracker tracker : trackers) { - TrackerFrame trackerFrame = tracker.safeGetFrame(cursor); - if ( - trackerFrame == null - || !trackerFrame.hasData(TrackerFrameData.POSITION) - || !trackerFrame.hasData(TrackerFrameData.DESIGNATION) - || trackerFrame.designation.trackerRole.isEmpty() - ) { - continue; - } - - ComputedTracker computedTracker = skeleton - .getComputedTracker(trackerFrame.designation.trackerRole.get()); - if (computedTracker != null) { - offset += FastMath.abs(computedTracker.position.distance(trackerFrame.position)); - offsetCount++; - } - } - - return offsetCount > 0 ? offset / offsetCount : 0f; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/PositionError.kt b/server/src/main/java/dev/slimevr/autobone/errors/PositionError.kt new file mode 100644 index 000000000..f9c86c7a4 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/PositionError.kt @@ -0,0 +1,58 @@ +package dev.slimevr.autobone.errors + +import com.jme3.math.FastMath +import dev.slimevr.autobone.AutoBoneTrainingStep +import dev.slimevr.poserecorder.PoseFrameTracker +import dev.slimevr.tracking.processor.skeleton.HumanSkeleton +import dev.slimevr.tracking.trackers.ComputedTracker + +// The distance of any points to the corresponding absolute position +class PositionError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + val trackers = trainingStep.trainingFrames.trackers + return ( + ( + getPositionError( + trackers, + trainingStep.cursor1, + trainingStep.humanPoseManager1.skeleton + ) + + getPositionError( + trackers, + trainingStep.cursor2, + trainingStep.humanPoseManager2.skeleton + ) + ) / + 2f + ) + } + + companion object { + fun getPositionError( + trackers: List, + cursor: Int, + skeleton: HumanSkeleton, + ): Float { + var offset = 0f + var offsetCount = 0 + for (tracker in trackers) { + val trackerFrame = tracker.safeGetFrame(cursor) + if (trackerFrame == null || + !trackerFrame.hasPosition() || + trackerFrame.bodyPosition == null || + trackerFrame.bodyPosition.trackerRole.isEmpty + ) { + continue + } + val computedTracker: ComputedTracker? = skeleton + .getComputedTracker(trackerFrame.bodyPosition.trackerRole.get()) + if (computedTracker != null) { + offset += FastMath.abs(computedTracker.position.distance(trackerFrame.position)) + offsetCount++ + } + } + return if (offsetCount > 0) offset / offsetCount else 0f + } + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java b/server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java deleted file mode 100644 index 6630f84be..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java +++ /dev/null @@ -1,81 +0,0 @@ -package dev.slimevr.autobone.errors; - -import java.util.List; - -import com.jme3.math.FastMath; - -import dev.slimevr.autobone.AutoBoneTrainingStep; -import dev.slimevr.poserecorder.PoseFrameTracker; -import dev.slimevr.poserecorder.TrackerFrame; -import dev.slimevr.poserecorder.TrackerFrameData; -import dev.slimevr.tracking.processor.skeleton.HumanSkeleton; -import dev.slimevr.tracking.trackers.ComputedTracker; - - -// The difference between offset of absolute position and the corresponding point over time -public class PositionOffsetError implements IAutoBoneError { - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - List trackers = trainingStep.getTrainingFrames().getTrackers(); - return getPositionOffsetError( - trackers, - trainingStep.getCursor1(), - trainingStep.getCursor2(), - trainingStep.getHumanPoseManager1().getSkeleton(), - trainingStep.getHumanPoseManager2().getSkeleton() - ); - } - - public float getPositionOffsetError( - List trackers, - int cursor1, - int cursor2, - HumanSkeleton skeleton1, - HumanSkeleton skeleton2 - ) { - float offset = 0f; - int offsetCount = 0; - - for (PoseFrameTracker tracker : trackers) { - TrackerFrame trackerFrame1 = tracker.safeGetFrame(cursor1); - if ( - trackerFrame1 == null - || !trackerFrame1.hasData(TrackerFrameData.POSITION) - || !trackerFrame1.hasData(TrackerFrameData.DESIGNATION) - || trackerFrame1.designation.trackerRole.isEmpty() - ) { - continue; - } - - TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2); - if ( - trackerFrame2 == null - || !trackerFrame2.hasData(TrackerFrameData.POSITION) - || !trackerFrame2.hasData(TrackerFrameData.DESIGNATION) - || trackerFrame2.designation.trackerRole.isEmpty() - ) { - continue; - } - - ComputedTracker computedTracker1 = skeleton1 - .getComputedTracker(trackerFrame1.designation.trackerRole.get()); - if (computedTracker1 == null) { - continue; - } - - ComputedTracker computedTracker2 = skeleton2 - .getComputedTracker(trackerFrame2.designation.trackerRole.get()); - if (computedTracker2 == null) { - continue; - } - - float dist1 = FastMath.abs(computedTracker1.position.distance(trackerFrame1.position)); - float dist2 = FastMath.abs(computedTracker2.position.distance(trackerFrame2.position)); - - offset += FastMath.abs(dist2 - dist1); - offsetCount++; - } - - return offsetCount > 0 ? offset / offsetCount : 0f; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.kt b/server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.kt new file mode 100644 index 000000000..ce7e6e4b1 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.kt @@ -0,0 +1,59 @@ +package dev.slimevr.autobone.errors + +import com.jme3.math.FastMath +import dev.slimevr.autobone.AutoBoneTrainingStep +import dev.slimevr.poserecorder.PoseFrameTracker +import dev.slimevr.tracking.processor.skeleton.HumanSkeleton + +// The difference between offset of absolute position and the corresponding point over time +class PositionOffsetError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + val trackers = trainingStep.trainingFrames.trackers + return getPositionOffsetError( + trackers, + trainingStep.cursor1, + trainingStep.cursor2, + trainingStep.humanPoseManager1.skeleton, + trainingStep.humanPoseManager2.skeleton + ) + } + + fun getPositionOffsetError( + trackers: List, + cursor1: Int, + cursor2: Int, + skeleton1: HumanSkeleton, + skeleton2: HumanSkeleton, + ): Float { + var offset = 0f + var offsetCount = 0 + for (tracker in trackers) { + val trackerFrame1 = tracker.safeGetFrame(cursor1) + if (trackerFrame1 == null || + !trackerFrame1.hasPosition() || + trackerFrame1.bodyPosition == null || + trackerFrame1.bodyPosition.trackerRole.isEmpty + ) { + continue + } + val trackerFrame2 = tracker.safeGetFrame(cursor2) + if (trackerFrame2 == null || + !trackerFrame2.hasPosition() || + trackerFrame2.bodyPosition == null || + trackerFrame2.bodyPosition.trackerRole.isEmpty + ) { + continue + } + val computedTracker1 = skeleton1 + .getComputedTracker(trackerFrame1.bodyPosition.trackerRole.get()) ?: continue + val computedTracker2 = skeleton2 + .getComputedTracker(trackerFrame2.bodyPosition.trackerRole.get()) ?: continue + val dist1 = FastMath.abs(computedTracker1.position.distance(trackerFrame1.position)) + val dist2 = FastMath.abs(computedTracker2.position.distance(trackerFrame2.position)) + offset += FastMath.abs(dist2 - dist1) + offsetCount++ + } + return if (offsetCount > 0) offset / offsetCount else 0f + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/SlideError.java b/server/src/main/java/dev/slimevr/autobone/errors/SlideError.java deleted file mode 100644 index a2d339550..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/SlideError.java +++ /dev/null @@ -1,41 +0,0 @@ -package dev.slimevr.autobone.errors; - -import dev.slimevr.autobone.AutoBoneTrainingStep; -import dev.slimevr.tracking.processor.skeleton.HumanSkeleton; -import dev.slimevr.tracking.trackers.ComputedTracker; -import dev.slimevr.tracking.trackers.TrackerRole; - - -// The change in position of the ankle over time -public class SlideError implements IAutoBoneError { - @Override - public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { - return getSlideError( - trainingStep.getHumanPoseManager1().getSkeleton(), - trainingStep.getHumanPoseManager2().getSkeleton() - ); - } - - public static float getSlideError(HumanSkeleton skeleton1, HumanSkeleton skeleton2) { - // Calculate and average between both feet - return (getSlideError(skeleton1, skeleton2, TrackerRole.LEFT_FOOT) - + getSlideError(skeleton1, skeleton2, TrackerRole.RIGHT_FOOT)) / 2f; - } - - public static float getSlideError( - HumanSkeleton skeleton1, - HumanSkeleton skeleton2, - TrackerRole trackerRole - ) { - // Calculate and average between both feet - return getSlideError( - skeleton1.getComputedTracker(trackerRole), - skeleton2.getComputedTracker(trackerRole) - ); - } - - public static float getSlideError(ComputedTracker tracker1, ComputedTracker tracker2) { - // Return the midpoint distance - return tracker1.position.distance(tracker2.position) / 2f; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/SlideError.kt b/server/src/main/java/dev/slimevr/autobone/errors/SlideError.kt new file mode 100644 index 000000000..e72305b09 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/SlideError.kt @@ -0,0 +1,44 @@ +package dev.slimevr.autobone.errors + +import dev.slimevr.autobone.AutoBoneTrainingStep +import dev.slimevr.tracking.processor.skeleton.HumanSkeleton +import dev.slimevr.tracking.trackers.ComputedTracker +import dev.slimevr.tracking.trackers.TrackerRole + +// The change in position of the ankle over time +class SlideError : IAutoBoneError { + @Throws(AutoBoneException::class) + override fun getStepError(trainingStep: AutoBoneTrainingStep): Float { + return getSlideError( + trainingStep.humanPoseManager1.skeleton, + trainingStep.humanPoseManager2.skeleton + ) + } + + companion object { + fun getSlideError(skeleton1: HumanSkeleton, skeleton2: HumanSkeleton): Float { + // Calculate and average between both feet + return ( + getSlideError(skeleton1, skeleton2, TrackerRole.LEFT_FOOT) + + getSlideError(skeleton1, skeleton2, TrackerRole.RIGHT_FOOT) + ) / 2f + } + + fun getSlideError( + skeleton1: HumanSkeleton, + skeleton2: HumanSkeleton, + trackerRole: TrackerRole, + ): Float { + // Calculate and average between both feet + return getSlideError( + skeleton1.getComputedTracker(trackerRole), + skeleton2.getComputedTracker(trackerRole) + ) + } + + fun getSlideError(tracker1: ComputedTracker, tracker2: ComputedTracker): Float { + // Return the midpoint distance + return tracker1.position.distance(tracker2.position) / 2f + } + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.java b/server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.java deleted file mode 100644 index c0aaec262..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.java +++ /dev/null @@ -1,36 +0,0 @@ -package dev.slimevr.autobone.errors.proportions; - -import dev.slimevr.tracking.processor.HumanPoseManager; - -import java.util.function.Function; - - -public class HardProportionLimiter implements ProportionLimiter { - - protected final float targetRatio; - protected final Function boneLengthFunction; - - /** - * @param targetRatio The bone to height ratio to target - * @param boneLengthFunction A function that takes a SkeletonConfig object - * and returns the bone length - */ - public HardProportionLimiter( - float targetRatio, - Function boneLengthFunction - ) { - this.targetRatio = targetRatio; - this.boneLengthFunction = boneLengthFunction; - } - - @Override - public float getProportionError(HumanPoseManager humanPoseManager, float height) { - float boneLength = boneLengthFunction.apply(humanPoseManager); - return targetRatio - (boneLength / height); - } - - @Override - public float getTargetRatio() { - return targetRatio; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.kt b/server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.kt new file mode 100644 index 000000000..33e427273 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/proportions/HardProportionLimiter.kt @@ -0,0 +1,19 @@ +package dev.slimevr.autobone.errors.proportions + +import dev.slimevr.tracking.processor.HumanPoseManager +import java.util.function.Function + +/** + * @param targetRatio The bone to height ratio to target + * @param boneLengthFunction A function that takes a SkeletonConfig object + * and returns the bone length + */ +open class HardProportionLimiter( + override val targetRatio: Float = 0f, + protected val boneLengthFunction: Function, +) : ProportionLimiter { + override fun getProportionError(humanPoseManager: HumanPoseManager, height: Float): Float { + val boneLength = boneLengthFunction.apply(humanPoseManager) + return targetRatio - boneLength / height + } +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.java b/server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.java deleted file mode 100644 index e8997db08..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.java +++ /dev/null @@ -1,10 +0,0 @@ -package dev.slimevr.autobone.errors.proportions; - -import dev.slimevr.tracking.processor.HumanPoseManager; - - -public interface ProportionLimiter { - public float getProportionError(HumanPoseManager humanPoseManager, float height); - - public float getTargetRatio(); -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.kt b/server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.kt new file mode 100644 index 000000000..1a963ddbf --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/proportions/ProportionLimiter.kt @@ -0,0 +1,8 @@ +package dev.slimevr.autobone.errors.proportions + +import dev.slimevr.tracking.processor.HumanPoseManager + +interface ProportionLimiter { + fun getProportionError(humanPoseManager: HumanPoseManager, height: Float): Float + val targetRatio: Float +} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.java b/server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.java deleted file mode 100644 index 666d83127..000000000 --- a/server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.java +++ /dev/null @@ -1,79 +0,0 @@ -package dev.slimevr.autobone.errors.proportions; - -import com.jme3.math.FastMath; -import dev.slimevr.tracking.processor.HumanPoseManager; - -import java.util.function.Function; - - -public class RangeProportionLimiter extends HardProportionLimiter { - - protected final float targetPositiveRange; - protected final float targetNegativeRange; - - /** - * @param targetRatio The bone to height ratio to target - * @param boneLengthFunction A function that takes a SkeletonConfig object - * and returns the bone length - * @param range The range from the target ratio to accept (ex. 0.1) - */ - public RangeProportionLimiter( - float targetRatio, - Function boneLengthFunction, - float range - ) { - super(targetRatio, boneLengthFunction); - - // Handle if someone puts in a negative value - range = FastMath.abs(range); - targetPositiveRange = range; - targetNegativeRange = -range; - } - - /** - * @param targetRatio The bone to height ratio to target - * @param boneLengthFunction A function that takes a SkeletonConfig object - * and returns the bone length - * @param positiveRange The positive range from the target ratio to accept - * (ex. 0.1) - * @param negativeRange The negative range from the target ratio to accept - * (ex. -0.1) - */ - public RangeProportionLimiter( - float targetRatio, - Function boneLengthFunction, - float positiveRange, - float negativeRange - ) { - super(targetRatio, boneLengthFunction); - - // If the positive range is less than the negative range, something is - // wrong - if (positiveRange < negativeRange) { - throw new IllegalArgumentException("positiveRange must not be less than negativeRange"); - } - - targetPositiveRange = positiveRange; - targetNegativeRange = negativeRange; - } - - @Override - public float getProportionError(HumanPoseManager config, float height) { - float boneLength = boneLengthFunction.apply(config); - float ratioOffset = targetRatio - (boneLength / height); - - // If the range is exceeded, return the offset from the range limit - if (ratioOffset > targetPositiveRange) { - return ratioOffset - targetPositiveRange; - } else if (ratioOffset < targetNegativeRange) { - return ratioOffset - targetNegativeRange; - } - - return 0f; - } - - @Override - public float getTargetRatio() { - return targetRatio; - } -} diff --git a/server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.kt b/server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.kt new file mode 100644 index 000000000..1d1a57578 --- /dev/null +++ b/server/src/main/java/dev/slimevr/autobone/errors/proportions/RangeProportionLimiter.kt @@ -0,0 +1,65 @@ +package dev.slimevr.autobone.errors.proportions + +import com.jme3.math.FastMath +import dev.slimevr.tracking.processor.HumanPoseManager +import java.util.function.Function + +class RangeProportionLimiter : HardProportionLimiter { + private val targetPositiveRange: Float + private val targetNegativeRange: Float + + /** + * @param targetRatio The bone to height ratio to target + * @param boneLengthFunction A function that takes a SkeletonConfig object + * and returns the bone length + * @param range The range from the target ratio to accept (ex. 0.1) + */ + constructor( + targetRatio: Float, + boneLengthFunction: Function, + range: Float, + ) : super(targetRatio, boneLengthFunction) { + var range = range + + // Handle if someone puts in a negative value + range = FastMath.abs(range) + targetPositiveRange = range + targetNegativeRange = -range + } + + /** + * @param targetRatio The bone to height ratio to target + * @param boneLengthFunction A function that takes a SkeletonConfig object + * and returns the bone length + * @param positiveRange The positive range from the target ratio to accept + * (ex. 0.1) + * @param negativeRange The negative range from the target ratio to accept + * (ex. -0.1) + */ + constructor( + targetRatio: Float, + boneLengthFunction: Function, + positiveRange: Float, + negativeRange: Float, + ) : super(targetRatio, boneLengthFunction) { + + // If the positive range is less than the negative range, something is + // wrong + require(positiveRange >= negativeRange) { "positiveRange must not be less than negativeRange" } + targetPositiveRange = positiveRange + targetNegativeRange = negativeRange + } + + override fun getProportionError(config: HumanPoseManager, height: Float): Float { + val boneLength = boneLengthFunction.apply(config) + val ratioOffset = targetRatio - boneLength / height + + // If the range is exceeded, return the offset from the range limit + if (ratioOffset > targetPositiveRange) { + return ratioOffset - targetPositiveRange + } else if (ratioOffset < targetNegativeRange) { + return ratioOffset - targetNegativeRange + } + return 0f + } +} diff --git a/server/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java b/server/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java index 1b9e52a80..289bd1df8 100644 --- a/server/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java +++ b/server/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java @@ -15,6 +15,21 @@ public final class PoseFrameIO { // Do not allow instantiating } + private static void writeVector3f(DataOutputStream outputStream, Vector3f vector) + throws IOException { + outputStream.writeFloat(vector.getX()); + outputStream.writeFloat(vector.getY()); + outputStream.writeFloat(vector.getZ()); + } + + private static void writeQuaternion(DataOutputStream outputStream, Quaternion quaternion) + throws IOException { + outputStream.writeFloat(quaternion.getX()); + outputStream.writeFloat(quaternion.getY()); + outputStream.writeFloat(quaternion.getZ()); + outputStream.writeFloat(quaternion.getW()); + } + public static boolean writeFrames(DataOutputStream outputStream, PoseFrames frames) { try { if (frames != null) { @@ -29,23 +44,35 @@ public final class PoseFrameIO { continue; } - outputStream.writeInt(trackerFrame.getDataFlags()); + int dataFlags = trackerFrame.getDataFlags(); - if (trackerFrame.hasData(TrackerFrameData.DESIGNATION)) { - outputStream.writeUTF(trackerFrame.designation.designation); + // Don't write destination strings anymore, replace with + // the enum + if (trackerFrame.hasData(TrackerFrameData.DESIGNATION_STRING)) { + dataFlags = TrackerFrameData.DESIGNATION_ENUM + .add(TrackerFrameData.DESIGNATION_STRING.remove(dataFlags)); } + outputStream.writeInt(dataFlags); + if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { - outputStream.writeFloat(trackerFrame.rotation.getX()); - outputStream.writeFloat(trackerFrame.rotation.getY()); - outputStream.writeFloat(trackerFrame.rotation.getZ()); - outputStream.writeFloat(trackerFrame.rotation.getW()); + writeQuaternion(outputStream, trackerFrame.rotation); } if (trackerFrame.hasData(TrackerFrameData.POSITION)) { - outputStream.writeFloat(trackerFrame.position.getX()); - outputStream.writeFloat(trackerFrame.position.getY()); - outputStream.writeFloat(trackerFrame.position.getZ()); + writeVector3f(outputStream, trackerFrame.position); + } + + if (TrackerFrameData.DESIGNATION_ENUM.check(dataFlags)) { + outputStream.writeInt(trackerFrame.designation.ordinal()); + } + + if (trackerFrame.hasData(TrackerFrameData.ACCELERATION)) { + writeVector3f(outputStream, trackerFrame.acceleration); + } + + if (trackerFrame.hasData(TrackerFrameData.RAW_ROTATION)) { + writeQuaternion(outputStream, trackerFrame.rawRotation); } } } @@ -75,6 +102,23 @@ public final class PoseFrameIO { return true; } + private static Vector3f readVector3f(DataInputStream inputStream) throws IOException { + return new Vector3f( + inputStream.readFloat(), + inputStream.readFloat(), + inputStream.readFloat() + ); + } + + private static Quaternion readQuaternion(DataInputStream inputStream) throws IOException { + return new Quaternion( + inputStream.readFloat(), + inputStream.readFloat(), + inputStream.readFloat(), + inputStream.readFloat() + ); + } + public static PoseFrames readFrames(DataInputStream inputStream) { try { @@ -91,7 +135,7 @@ public final class PoseFrameIO { int dataFlags = inputStream.readInt(); TrackerPosition designation = null; - if (TrackerFrameData.DESIGNATION.check(dataFlags)) { + if (TrackerFrameData.DESIGNATION_STRING.check(dataFlags)) { designation = TrackerPosition .getByDesignation(inputStream.readUTF()) .orElse(null); @@ -99,22 +143,38 @@ public final class PoseFrameIO { Quaternion rotation = null; if (TrackerFrameData.ROTATION.check(dataFlags)) { - float quatX = inputStream.readFloat(); - float quatY = inputStream.readFloat(); - float quatZ = inputStream.readFloat(); - float quatW = inputStream.readFloat(); - rotation = new Quaternion(quatX, quatY, quatZ, quatW); + rotation = readQuaternion(inputStream); } Vector3f position = null; if (TrackerFrameData.POSITION.check(dataFlags)) { - float posX = inputStream.readFloat(); - float posY = inputStream.readFloat(); - float posZ = inputStream.readFloat(); - position = new Vector3f(posX, posY, posZ); + position = readVector3f(inputStream); } - trackerFrames.add(new TrackerFrame(designation, rotation, position)); + if (TrackerFrameData.DESIGNATION_ENUM.check(dataFlags)) { + designation = TrackerPosition.values[inputStream.readInt()]; + } + + Vector3f acceleration = null; + if (TrackerFrameData.ACCELERATION.check(dataFlags)) { + acceleration = readVector3f(inputStream); + } + + Quaternion rawRotation = null; + if (TrackerFrameData.RAW_ROTATION.check(dataFlags)) { + rawRotation = readQuaternion(inputStream); + } + + trackerFrames + .add( + new TrackerFrame( + designation, + rotation, + position, + acceleration, + rawRotation + ) + ); } trackers.add(new PoseFrameTracker(name, trackerFrames)); diff --git a/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java b/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java index f6ad3a493..01fd9f938 100644 --- a/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java +++ b/server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java @@ -145,9 +145,8 @@ public class PoseFrameTracker implements Tracker, Iterable { @Override public boolean getRotation(Quaternion store) { TrackerFrame frame = safeGetFrame(); - if (frame != null && frame.hasData(TrackerFrameData.ROTATION)) { - store.set(frame.rotation); - return true; + if (frame != null) { + return frame.getRotation(store); } store.set(Quaternion.IDENTITY); @@ -157,9 +156,8 @@ public class PoseFrameTracker implements Tracker, Iterable { @Override public boolean getRawRotation(Quaternion store) { TrackerFrame frame = safeGetFrame(); - if (frame != null && frame.hasData(TrackerFrameData.ROTATION)) { - store.set(frame.rotation); - return true; + if (frame != null) { + return frame.getRawRotation(store); } store.set(Quaternion.IDENTITY); @@ -169,9 +167,8 @@ public class PoseFrameTracker implements Tracker, Iterable { @Override public boolean getPosition(Vector3f store) { TrackerFrame frame = safeGetFrame(); - if (frame != null && frame.hasData(TrackerFrameData.POSITION)) { - store.set(frame.position); - return true; + if (frame != null) { + return frame.getPosition(store); } store.set(Vector3f.ZERO); @@ -180,7 +177,12 @@ public class PoseFrameTracker implements Tracker, Iterable { @Override public boolean getAcceleration(Vector3f store) { - store.set(0, 0, 0); + TrackerFrame frame = safeGetFrame(); + if (frame != null) { + return frame.getAcceleration(store); + } + + store.set(Vector3f.ZERO); return false; } diff --git a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java index efa561a99..a589c4bb3 100644 --- a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java +++ b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java @@ -14,23 +14,41 @@ public final class TrackerFrame implements Tracker { public final TrackerPosition designation; public final Quaternion rotation; public final Vector3f position; + public final Vector3f acceleration; + public final Quaternion rawRotation; private final int trackerId = Tracker.getNextLocalTrackerId(); private int dataFlags = 0; - public TrackerFrame(TrackerPosition designation, Quaternion rotation, Vector3f position) { + public TrackerFrame( + TrackerPosition designation, + Quaternion rotation, + Vector3f position, + Vector3f acceleration, + Quaternion rawRotation + ) { this.designation = designation; if (designation != null) { - dataFlags |= TrackerFrameData.DESIGNATION.flag; + dataFlags = TrackerFrameData.DESIGNATION_ENUM.add(dataFlags); } this.rotation = rotation; if (rotation != null) { - dataFlags |= TrackerFrameData.ROTATION.flag; + dataFlags = TrackerFrameData.ROTATION.add(dataFlags); } this.position = position; if (position != null) { - dataFlags |= TrackerFrameData.POSITION.flag; + dataFlags = TrackerFrameData.POSITION.add(dataFlags); + } + + this.acceleration = acceleration; + if (acceleration != null) { + dataFlags = TrackerFrameData.ACCELERATION.add(dataFlags); + } + + this.rawRotation = rawRotation; + if (rawRotation != null) { + dataFlags = TrackerFrameData.RAW_ROTATION.add(dataFlags); } } @@ -48,16 +66,13 @@ public final class TrackerFrame implements Tracker { return null; } - // If tracker has no data - if (tracker.getBodyPosition() == null && !tracker.hasRotation() && !tracker.hasPosition()) { - return null; - } + TrackerPosition designation = tracker.getBodyPosition(); Quaternion rotation = null; if (tracker.hasRotation()) { rotation = new Quaternion(); if (!tracker.getRotation(rotation)) { - // If getting the rotation failed, set it back to null + // If the get failed, set it back to null rotation = null; } } @@ -66,12 +81,47 @@ public final class TrackerFrame implements Tracker { if (tracker.hasPosition()) { position = new Vector3f(); if (!tracker.getPosition(position)) { - // If getting the position failed, set it back to null + // If the get failed, set it back to null position = null; } } - return new TrackerFrame(tracker.getBodyPosition(), rotation, position); + Vector3f acceleration = null; + if (tracker.hasAcceleration()) { + acceleration = new Vector3f(); + if (!tracker.getAcceleration(acceleration)) { + // If the get failed, set it back to null + acceleration = null; + } + } + + // TODO: Why is there no `hasRawRotation`? Update this to check that + // first when it exists + Quaternion rawRotation = new Quaternion(); + if (!(tracker.getRawRotation(rawRotation) && !rawRotation.equals(rotation))) { + // If the get failed or the rawRotation is the same as rotation, set + // it back to null + rawRotation = null; + } + + // If tracker has no data at all, there's no point in writing a frame + if ( + designation == null + && rotation == null + && position == null + && acceleration == null + && rawRotation == null + ) { + return null; + } + + return new TrackerFrame( + designation, + rotation, + position, + acceleration, + rawRotation + ); } public int getDataFlags() { @@ -96,13 +146,12 @@ public final class TrackerFrame implements Tracker { @Override public boolean getRawRotation(Quaternion store) { - if (hasData(TrackerFrameData.ROTATION)) { - store.set(rotation); + if (hasData(TrackerFrameData.RAW_ROTATION)) { + store.set(rawRotation); return true; } - store.set(Quaternion.IDENTITY); - return false; + return getRotation(store); } @Override @@ -118,7 +167,12 @@ public final class TrackerFrame implements Tracker { @Override public boolean getAcceleration(Vector3f store) { - store.set(0, 0, 0); + if (hasData(TrackerFrameData.ACCELERATION)) { + store.set(acceleration); + return true; + } + + store.set(Vector3f.ZERO); return false; } diff --git a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.java b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.java deleted file mode 100644 index e000a25e0..000000000 --- a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.java +++ /dev/null @@ -1,16 +0,0 @@ -package dev.slimevr.poserecorder; - -public enum TrackerFrameData { - - DESIGNATION(0), ROTATION(1), POSITION(2),; - - public final int flag; - - TrackerFrameData(int id) { - this.flag = 1 << id; - } - - public boolean check(int dataFlags) { - return (dataFlags & this.flag) != 0; - } -} diff --git a/server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.kt b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.kt new file mode 100644 index 000000000..68609f618 --- /dev/null +++ b/server/src/main/java/dev/slimevr/poserecorder/TrackerFrameData.kt @@ -0,0 +1,23 @@ +package dev.slimevr.poserecorder + +enum class TrackerFrameData(id: Int) { + DESIGNATION_STRING(0), ROTATION(1), POSITION(2), DESIGNATION_ENUM(3), ACCELERATION(4), RAW_ROTATION(5); + + val flag: Int + + init { + flag = 1 shl id + } + + inline fun check(dataFlags: Int): Boolean { + return dataFlags and flag != 0 + } + + inline fun add(dataFlags: Int): Int { + return dataFlags or flag + } + + inline fun remove(dataFlags: Int): Int { + return dataFlags xor flag + } +} diff --git a/server/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.java b/server/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.java index 79d72dd7e..dffac9ecc 100644 --- a/server/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.java +++ b/server/src/main/java/dev/slimevr/posestreamer/PoseFrameStreamer.java @@ -30,6 +30,10 @@ public class PoseFrameStreamer extends PoseStreamer { return frames; } + public HumanPoseManager getHumanPoseManager() { + return humanPoseManager; + } + public synchronized void streamAllFrames() { for (int i = 0; i < frames.getMaxFrameCount(); i++) { frames.setCursors(i); diff --git a/server/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java b/server/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java index b2ac86bcd..ec992af95 100644 --- a/server/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java +++ b/server/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.java @@ -298,7 +298,7 @@ public class RPCHandler extends ProtocolHandler conn.getContext().setUseAutoBone(true); this.api.server .getAutoBoneHandler() - .startProcessByType(AutoBoneProcessType.getById(req.processType())); + .startProcessByType(AutoBoneProcessType.Companion.getById(req.processType())); } @Override @@ -322,7 +322,7 @@ public class RPCHandler extends ProtocolHandler Integer messageOffset = message != null ? fbb.createString(message) : null; AutoBoneProcessStatusResponse.startAutoBoneProcessStatusResponse(fbb); - AutoBoneProcessStatusResponse.addProcessType(fbb, processType.id); + AutoBoneProcessStatusResponse.addProcessType(fbb, processType.getId()); if (messageOffset != null) AutoBoneProcessStatusResponse.addMessage(fbb, messageOffset); if (total > 0 && current >= 0) { @@ -365,10 +365,11 @@ public class RPCHandler extends ProtocolHandler .forEach((conn) -> { FlatBufferBuilder fbb = new FlatBufferBuilder(32); - int[] skeletonPartOffsets = new int[epoch.configValues.size()]; + int[] skeletonPartOffsets = new int[epoch.getConfigValues().size()]; int i = 0; for ( - Entry skeletonConfig : epoch.configValues + Entry skeletonConfig : epoch + .getConfigValues() .entrySet() ) { skeletonPartOffsets[i++] = SkeletonPart @@ -385,9 +386,9 @@ public class RPCHandler extends ProtocolHandler int update = AutoBoneEpochResponse .createAutoBoneEpochResponse( fbb, - epoch.epoch, - epoch.totalEpochs, - epoch.epochError.getMean(), + epoch.getEpoch(), + epoch.getTotalEpochs(), + epoch.getEpochError().getMean(), skeletonPartsOffset ); int outbound = this diff --git a/server/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.java b/server/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.java index a4b078afd..67b14def4 100644 --- a/server/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.java +++ b/server/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.java @@ -3,6 +3,7 @@ package dev.slimevr.tracking.processor.config; import com.jme3.math.Vector3f; import dev.slimevr.Main; import dev.slimevr.autobone.errors.BodyProportionError; +import dev.slimevr.autobone.errors.proportions.ProportionLimiter; import dev.slimevr.config.ConfigManager; import dev.slimevr.tracking.processor.BoneType; import dev.slimevr.tracking.processor.HumanPoseManager; @@ -410,13 +411,16 @@ public class SkeletonConfigManager { float height = humanPoseManager.getHmdHeight() / BodyProportionError.eyeHeightToHeightRatio; if (height > 0.5f) { // Reset only if floor level seems right, - setOffset( - config, - height - * BodyProportionError - .getProportionLimitForOffset(config) - .getTargetRatio() - ); + ProportionLimiter proportionLimiter = BodyProportionError + .getProportionLimitForOffset(config); + if (proportionLimiter != null) { + setOffset( + config, + height * proportionLimiter.getTargetRatio() + ); + } else { + setOffset(config, null); + } } else { // if floor level is incorrect setOffset(config, null); }