From b1318567e3ead612ea754160bc60f19efa6efe0f Mon Sep 17 00:00:00 2001 From: Butterscotch! Date: Tue, 28 Jun 2022 15:00:57 -0400 Subject: [PATCH] AutoBone: Adjust bone offsets for directly and implement new contribution based error calculation (#204) --- .../java/dev/slimevr/autobone/AutoBone.java | 1002 ++++++++--------- .../dev/slimevr/autobone/AutoBoneHandler.java | 83 +- .../autobone/AutoBoneTrainingStep.java | 108 ++ .../autobone/errors/AutoBoneException.java | 28 + .../autobone/errors/BodyProportionError.java | 53 + .../errors/FootHeightOffsetError.java | 54 + .../slimevr/autobone/errors/HeightError.java | 21 + .../autobone/errors/IAutoBoneError.java | 8 + .../autobone/errors/OffsetSlideError.java | 61 + .../autobone/errors/PositionError.java | 61 + .../autobone/errors/PositionOffsetError.java | 79 ++ .../slimevr/autobone/errors/SlideError.java | 38 + .../vr/processor/skeleton/HumanSkeleton.java | 55 + 13 files changed, 1120 insertions(+), 531 deletions(-) create mode 100644 src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/HeightError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/PositionError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java create mode 100644 src/main/java/dev/slimevr/autobone/errors/SlideError.java diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index 3166e337fd..db1f35c2a1 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -3,14 +3,23 @@ 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.poserecorder.*; import dev.slimevr.vr.processor.HumanPoseProcessor; +import dev.slimevr.vr.processor.TransformNode; +import dev.slimevr.vr.processor.skeleton.BoneType; import dev.slimevr.vr.processor.skeleton.Skeleton; +import dev.slimevr.vr.processor.skeleton.HumanSkeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfig; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; -import dev.slimevr.vr.trackers.TrackerPosition; import dev.slimevr.vr.trackers.TrackerRole; -import dev.slimevr.vr.trackers.TrackerUtils; import io.eiren.util.StringUtils; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; @@ -20,8 +29,11 @@ import java.util.EnumMap; import java.util.List; import java.util.Map; +import java.util.Random; import java.util.Map.Entry; +import java.util.function.BiConsumer; import java.util.function.Consumer; +import java.util.function.Function; public class AutoBone { @@ -29,41 +41,88 @@ public class AutoBone { private static final File saveDir = new File("Recordings"); private static final File loadDir = new File("LoadRecordings"); // This is filled by reloadConfigValues() - public final EnumMap configs = new EnumMap( - SkeletonConfigValue.class + public final EnumMap offsets = new EnumMap( + BoneType.class ); - public final EnumMap staticConfigs = new EnumMap( - SkeletonConfigValue.class + + public final FastList adjustOffsets = new FastList( + new BoneType[] { + BoneType.HEAD, + BoneType.NECK, + BoneType.CHEST, + BoneType.WAIST, + BoneType.HIP, + + // This one doesn't seem to work very well and is generally going to + // be similar between users + // 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 heightConfigs = new FastList( - new SkeletonConfigValue[] { SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, - SkeletonConfigValue.LEGS_LENGTH } + + public final FastList legacyHeightConfigs = new FastList( + new SkeletonConfigValue[] { + SkeletonConfigValue.NECK, + SkeletonConfigValue.TORSO, + + SkeletonConfigValue.LEGS_LENGTH, + } ); - public final FastList lengthConfigs = new FastList( - new SkeletonConfigValue[] { SkeletonConfigValue.HEAD, SkeletonConfigValue.NECK, - SkeletonConfigValue.TORSO, SkeletonConfigValue.HIPS_WIDTH, - SkeletonConfigValue.LEGS_LENGTH } + + public final EnumMap legacyConfigs = new EnumMap( + SkeletonConfigValue.class ); + protected final VRServer server; - public int cursorIncrement = 1; - public int minDataDistance = 2; - public int maxDataDistance = 32; - public int numEpochs = 5; - public float initialAdjustRate = 2.5f; - public float adjustRateDecay = 1.01f; - public float slideErrorFactor = 1.0f; - public float offsetSlideErrorFactor = 0.0f; - public float offsetErrorFactor = 0.0f; - public float proportionErrorFactor = 0.2f; - public float heightErrorFactor = 0.1f; - - // TODO Needs much more work, probably going to rethink how the errors work - // to - // avoid this barely functional workaround @ButterscotchV - // For scaling distances, since smaller sizes will cause smaller distances - // private float totalLengthBase = 2f; + public int cursorIncrement = 2; + public int minDataDistance = 1; + public int maxDataDistance = 1; + public int numEpochs = 100; + public float initialAdjustRate = 10f; + public float adjustRateMultiplier = 0.995f; + + // #region Error functions + public SlideError slideError = new SlideError(); + public float slideErrorFactor = 0.0f; + + public OffsetSlideError offsetSlideError = new OffsetSlideError(); + public float offsetSlideErrorFactor = 1.0f; + + public FootHeightOffsetError footHeightOffsetError = new FootHeightOffsetError(); + public float footHeightOffsetErrorFactor = 0.0f; + + public BodyProportionError bodyProportionError = new BodyProportionError(); + public float bodyProportionErrorFactor = 0.2f; + + public HeightError heightError = new HeightError(); + public float heightErrorFactor = 0.0f; + + public PositionError positionError = new PositionError(); public float positionErrorFactor = 0.0f; + + public PositionOffsetError positionOffsetError = new PositionOffsetError(); public float positionOffsetErrorFactor = 0.0f; + // #endregion + + public boolean randomizeFrameOrder = true; + public boolean scaleEachStep = true; + public boolean calcInitError = false; public float targetHeight = -1; @@ -84,6 +143,8 @@ public class AutoBone { // additional hip tracker. public float chestTorsoRatio = 0.57f; + private final Random rand = new Random(); + public AutoBone(VRServer server) { this.server = server; reloadConfigValues(); @@ -97,17 +158,17 @@ public AutoBone(VRServer server) { this.initialAdjustRate = server.config .getFloat("autobone.adjustRate", this.initialAdjustRate); - this.adjustRateDecay = server.config - .getFloat("autobone.adjustRateDecay", this.adjustRateDecay); + this.adjustRateMultiplier = server.config + .getFloat("autobone.adjustRateMultiplier", this.adjustRateMultiplier); this.slideErrorFactor = server.config .getFloat("autobone.slideErrorFactor", this.slideErrorFactor); this.offsetSlideErrorFactor = server.config .getFloat("autobone.offsetSlideErrorFactor", this.offsetSlideErrorFactor); - this.offsetErrorFactor = server.config - .getFloat("autobone.offsetErrorFactor", this.offsetErrorFactor); - this.proportionErrorFactor = server.config - .getFloat("autobone.proportionErrorFactor", this.proportionErrorFactor); + this.footHeightOffsetErrorFactor = server.config + .getFloat("autobone.offsetErrorFactor", this.footHeightOffsetErrorFactor); + this.bodyProportionErrorFactor = server.config + .getFloat("autobone.proportionErrorFactor", this.bodyProportionErrorFactor); this.heightErrorFactor = server.config .getFloat("autobone.heightErrorFactor", this.heightErrorFactor); this.positionErrorFactor = server.config @@ -132,70 +193,63 @@ public void reloadConfigValues() { reloadConfigValues(null); } - private float readFromConfig(SkeletonConfigValue configValue) { - return server.config.getFloat(configValue.configKey, configValue.defaultValue); + public void reloadConfigValues(List trackers) { + for (BoneType offset : adjustOffsets) { + offsets.put(offset, 0.4f); + } } - public void reloadConfigValues(List trackers) { - // Load torso configs - staticConfigs.put(SkeletonConfigValue.HEAD, readFromConfig(SkeletonConfigValue.HEAD)); - staticConfigs.put(SkeletonConfigValue.NECK, readFromConfig(SkeletonConfigValue.NECK)); - configs.put(SkeletonConfigValue.TORSO, readFromConfig(SkeletonConfigValue.TORSO)); - if ( - server.config.getBoolean("autobone.forceChestTracker", false) - || (trackers != null - && TrackerUtils - .findNonComputedHumanPoseTrackerForBodyPosition( - trackers, - TrackerPosition.CHEST - ) - != null) - ) { - // If force enabled or has a chest tracker - staticConfigs.remove(SkeletonConfigValue.CHEST); - configs.put(SkeletonConfigValue.CHEST, readFromConfig(SkeletonConfigValue.CHEST)); - } else { - // Otherwise, make sure it's not used - configs.remove(SkeletonConfigValue.CHEST); - staticConfigs.put(SkeletonConfigValue.CHEST, readFromConfig(SkeletonConfigValue.CHEST)); + public Vector3f getBoneDirection( + HumanSkeleton skeleton, + BoneType node, + boolean rightSide, + Vector3f buffer + ) { + if (buffer == null) { + buffer = new Vector3f(); } - if ( - server.config.getBoolean("autobone.forceHipTracker", false) - || (trackers != null - && TrackerUtils - .findNonComputedHumanPoseTrackerForBodyPosition( - trackers, - TrackerPosition.HIP - ) - != null - && TrackerUtils - .findNonComputedHumanPoseTrackerForBodyPosition( - trackers, - TrackerPosition.WAIST - ) - != null) - ) { - // If force enabled or has a hip tracker and waist tracker - staticConfigs.remove(SkeletonConfigValue.WAIST); - configs.put(SkeletonConfigValue.WAIST, readFromConfig(SkeletonConfigValue.WAIST)); - } else { - // Otherwise, make sure it's not used - configs.remove(SkeletonConfigValue.WAIST); - staticConfigs.put(SkeletonConfigValue.WAIST, readFromConfig(SkeletonConfigValue.WAIST)); + + switch (node) { + case LEFT_HIP: + case RIGHT_HIP: + node = rightSide ? BoneType.RIGHT_HIP : BoneType.LEFT_HIP; + break; + + case LEFT_UPPER_LEG: + case RIGHT_UPPER_LEG: + node = rightSide ? BoneType.RIGHT_UPPER_LEG : BoneType.LEFT_UPPER_LEG; + break; + + case LEFT_LOWER_LEG: + case RIGHT_LOWER_LEG: + node = rightSide ? BoneType.RIGHT_LOWER_LEG : BoneType.LEFT_LOWER_LEG; + break; } - // Load leg configs - staticConfigs - .put(SkeletonConfigValue.HIPS_WIDTH, readFromConfig(SkeletonConfigValue.HIPS_WIDTH)); - configs - .put(SkeletonConfigValue.LEGS_LENGTH, readFromConfig(SkeletonConfigValue.LEGS_LENGTH)); - configs - .put(SkeletonConfigValue.KNEE_HEIGHT, readFromConfig(SkeletonConfigValue.KNEE_HEIGHT)); + TransformNode relevantTransform = skeleton.getNode(node); + return relevantTransform.worldTransform + .getTranslation() + .subtract(relevantTransform.getParent().worldTransform.getTranslation(), buffer) + .normalizeLocal(); + } + + public float getDotProductDiff( + HumanSkeleton skeleton1, + HumanSkeleton 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); - // Keep "feet" at ankles - staticConfigs.put(SkeletonConfigValue.FOOT_LENGTH, 0f); - staticConfigs.put(SkeletonConfigValue.FOOT_SHIFT, 0f); - staticConfigs.put(SkeletonConfigValue.SKELETON_OFFSET, 0f); + return dot2 - dot1; } /** @@ -211,71 +265,138 @@ private Skeleton getSkeleton() { return humanPoseProcessor != null ? humanPoseProcessor.getSkeleton() : null; } - public void applyConfig() { - if (!applyConfigToSkeleton(getSkeleton())) { + public void applyAndSaveConfig() { + if (!applyAndSaveConfig(getSkeleton())) { // Unable to apply to skeleton, save directly - saveConfigs(); + // saveConfigs(); } } - public boolean applyConfigToSkeleton(Skeleton skeleton) { - if (skeleton == null) { + public boolean applyConfig( + BiConsumer configConsumer, + Map offsets + ) { + if (configConsumer == null || offsets == null) { return false; } - SkeletonConfig skeletonConfig = skeleton.getSkeletonConfig(); - skeletonConfig.setConfigs(configs, null); - skeletonConfig.saveToConfig(server.config); - server.saveConfig(); + try { + Float headOffset = offsets.get(BoneType.HEAD); + if (headOffset != null) { + configConsumer.accept(SkeletonConfigValue.HEAD, headOffset); + } - LogManager.info("[AutoBone] Configured skeleton bone lengths"); - return true; + Float neckOffset = offsets.get(BoneType.NECK); + if (neckOffset != null) { + configConsumer.accept(SkeletonConfigValue.NECK, neckOffset); + } + + Float chestOffset = offsets.get(BoneType.CHEST); + Float hipOffset = offsets.get(BoneType.HIP); + Float waistOffset = offsets.get(BoneType.WAIST); + if (chestOffset != null && hipOffset != null && waistOffset != null) { + configConsumer + .accept(SkeletonConfigValue.TORSO, chestOffset + hipOffset + waistOffset); + } + + if (chestOffset != null) { + configConsumer.accept(SkeletonConfigValue.CHEST, chestOffset); + } + + if (hipOffset != null) { + configConsumer.accept(SkeletonConfigValue.WAIST, hipOffset); + } + + Float hipWidthOffset = offsets.get(BoneType.LEFT_HIP); + if (hipWidthOffset == null) { + hipWidthOffset = offsets.get(BoneType.RIGHT_HIP); + } + if (hipWidthOffset != null) { + configConsumer + .accept(SkeletonConfigValue.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 && lowerLegOffset != null) { + configConsumer + .accept(SkeletonConfigValue.LEGS_LENGTH, upperLegOffset + lowerLegOffset); + } + + if (lowerLegOffset != null) { + configConsumer.accept(SkeletonConfigValue.KNEE_HEIGHT, lowerLegOffset); + } + return true; + } catch (Exception e) { + return false; + } } - private void setConfig(SkeletonConfigValue config) { - Float value = configs.get(config); - if (value != null) { - server.config.setProperty(config.configKey, value); + 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); } - // This doesn't require a skeleton, therefore can be used if skeleton is - // null - public void saveConfigs() { - for (SkeletonConfigValue config : SkeletonConfigValue.values) { - setConfig(config); + public boolean applyConfig(SkeletonConfig skeletonConfig, Map offsets) { + if (skeletonConfig == null) { + return false; } - server.saveConfig(); + return applyConfig(skeletonConfig::setConfig, offsets); } - public Float getConfig(SkeletonConfigValue config) { - Float configVal = configs.get(config); - return configVal != null ? configVal : staticConfigs.get(config); + public boolean applyConfig(SkeletonConfig skeletonConfig) { + return applyConfig(skeletonConfig, offsets); } - public Float getConfig( - SkeletonConfigValue config, - Map configs, - Map configsAlt - ) { - if (configs == null) { - throw new NullPointerException("Argument \"configs\" must not be null"); + public boolean applyAndSaveConfig(Skeleton skeleton) { + if (skeleton == null) { + return false; } - Float configVal = configs.get(config); - return configVal != null || configsAlt == null ? configVal : configsAlt.get(config); + SkeletonConfig skeletonConfig = skeleton.getSkeletonConfig(); + if (!applyConfig(skeletonConfig)) + return false; + + skeletonConfig.saveToConfig(server.config); + server.saveConfig(); + + LogManager.info("[AutoBone] Configured skeleton bone lengths"); + return true; } - public float sumSelectConfigs( - List selection, - Map configs, - Map configsAlt + public Float getConfig(BoneType config) { + return offsets.get(config); + } + + public float sumSelectConfigs( + List selection, + Function configs ) { float sum = 0f; - for (SkeletonConfigValue config : selection) { - Float length = getConfig(config, configs, configsAlt); + for (T config : selection) { + Float length = configs.apply(config); if (length != null) { sum += length; } @@ -284,31 +405,32 @@ public float sumSelectConfigs( return sum; } + public float sumSelectConfigs( + List selection, + Map configs + ) { + return sumSelectConfigs(selection, configs::get); + } + public float sumSelectConfigs( List selection, - SkeletonConfig skeletonConfig + SkeletonConfig config ) { - float sum = 0f; - - for (SkeletonConfigValue config : selection) { - sum += skeletonConfig.getConfig(config); - } - - return sum; + return sumSelectConfigs(selection, config::getConfig); } - public float getLengthSum(Map configs) { + public float getLengthSum(Map configs) { return getLengthSum(configs, null); } public float getLengthSum( - Map configs, - Map configsAlt + Map configs, + Map configsAlt ) { float length = 0f; if (configsAlt != null) { - for (Entry config : configsAlt.entrySet()) { + for (Entry config : configsAlt.entrySet()) { // If there isn't a duplicate config if (!configs.containsKey(config.getKey())) { length += config.getValue(); @@ -323,24 +445,59 @@ public float getLengthSum( return length; } - public void processFrames(PoseFrames frames, Consumer epochCallback) { - processFrames(frames, -1f, epochCallback); + public float getTargetHeight(PoseFrames frames) { + float targetHeight; + // Get the current skeleton from the server + Skeleton skeleton = getSkeleton(); + if (skeleton != null) { + // If there is a skeleton available, calculate the target height + // from its configs + targetHeight = sumSelectConfigs(legacyHeightConfigs, skeleton.getSkeletonConfig()); + 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 void processFrames( + public AutoBoneResults processFrames( PoseFrames frames, float targetHeight, Consumer epochCallback - ) { - processFrames(frames, true, targetHeight, epochCallback); + ) throws AutoBoneException { + return processFrames(frames, true, targetHeight, epochCallback); } - public float processFrames( + public AutoBoneResults processFrames( PoseFrames frames, boolean calcInitError, float targetHeight, Consumer epochCallback - ) { + ) throws AutoBoneException { final int frameCount = frames.getMaxFrameCount(); List trackers = frames.getTrackers(); @@ -349,49 +506,28 @@ public float processFrames( final PoseFrameSkeleton skeleton1 = new PoseFrameSkeleton( trackers, - null, - configs, - staticConfigs + null ); final PoseFrameSkeleton skeleton2 = new PoseFrameSkeleton( trackers, - null, - configs, - staticConfigs + null + ); + + EnumMap intermediateOffsets = new EnumMap( + offsets + ); + + AutoBoneTrainingStep trainingStep = new AutoBoneTrainingStep( + targetHeight, + skeleton1, + skeleton2, + frames, + intermediateOffsets ); // If target height isn't specified, auto-detect if (targetHeight < 0f) { - // Get the current skeleton from the server - Skeleton skeleton = getSkeleton(); - if (skeleton != null) { - // If there is a skeleton available, calculate the target height - // from its - // configs - targetHeight = sumSelectConfigs(heightConfigs, skeleton.getSkeletonConfig()); - 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; - } + targetHeight = getTargetHeight(frames); } // Epoch loop, each epoch is one full iteration over the full dataset @@ -399,15 +535,37 @@ public float processFrames( float sumError = 0f; int errorCount = 0; - float adjustRate = epoch - >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f; + float adjustRate = epoch >= 0 + ? (initialAdjustRate * FastMath.pow(adjustRateMultiplier, epoch)) + : 0f; + + int[] randomFrameIndices = null; + if (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 = minDataDistance; - cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++ + int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance + && cursorOffset < frameCount; + cursorOffset++ ) { for ( int frameCursor = 0; frameCursor < frameCount - cursorOffset; @@ -415,28 +573,30 @@ public float processFrames( ) { int frameCursor2 = frameCursor + cursorOffset; - skeleton1.skeletonConfig.setConfigs(configs, null); - skeleton2.skeletonConfig.setConfigs(configs, null); + applyConfig(skeleton1.skeletonConfig); + skeleton2.skeletonConfig.setConfigs(skeleton1.skeletonConfig); - skeleton1.setCursor(frameCursor); - skeleton1.updatePose(); + if (randomizeFrameOrder) { + trainingStep + .setCursors( + randomFrameIndices[frameCursor], + randomFrameIndices[frameCursor2] + ); + } else { + trainingStep.setCursors(frameCursor, frameCursor2); + } + + skeleton1.setCursor(trainingStep.getCursor1()); + skeleton2.setCursor(trainingStep.getCursor2()); - skeleton2.setCursor(frameCursor2); + skeleton1.updatePose(); skeleton2.updatePose(); - float totalLength = getLengthSum(configs); - float curHeight = sumSelectConfigs(heightConfigs, configs, staticConfigs); - // float scaleLength = sumSelectConfigs(lengthConfigs, - // configs, staticConfigs); - float errorDeriv = getErrorDeriv( - frames, - frameCursor, - frameCursor2, - skeleton1, - skeleton2, - targetHeight - curHeight, - 1f - ); + float totalLength = getLengthSum(offsets); + float curHeight = sumSelectConfigs(heightOffsets, offsets); + trainingStep.setCurrentHeight(curHeight); + + float errorDeriv = getErrorDeriv(trainingStep); float error = errorFunc(errorDeriv); // In case of fire @@ -467,7 +627,21 @@ public float processFrames( continue; } - for (Entry entry : configs.entrySet()) { + 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) { @@ -475,361 +649,164 @@ public float processFrames( } float originalLength = entry.getValue(); + boolean isHeightVar = heightOffsets.contains(entry.getKey()); - // Try positive and negative adjustments - boolean isHeightVar = heightConfigs.contains(entry.getKey()); - // boolean isLengthVar = - // lengthConfigs.contains(entry.getKey()); - float minError = errorDeriv; - float finalNewLength = -1f; - for (int i = 0; i < 2; i++) { - // Scale by the ratio for smooth adjustment and more - // stable results - float curAdjustVal = ((i == 0 ? adjustVal : -adjustVal) - * originalLength) / totalLength; - float newLength = originalLength + curAdjustVal; - - // No small or negative numbers!!! Bad algorithm! - if (newLength < 0.01f) { - continue; - } - - updateSkeletonBoneLength( - skeleton1, - skeleton2, - entry.getKey(), - newLength - ); - - float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight; - // float newScaleLength = isLengthVar ? scaleLength - // + curAdjustVal : - // scaleLength; - float newErrorDeriv = getErrorDeriv( - frames, - frameCursor, - frameCursor2, - skeleton1, - skeleton2, - targetHeight - newHeight, - 1f - ); - - if (newErrorDeriv < minError) { - minError = newErrorDeriv; - finalNewLength = newLength; - } - } - - if (finalNewLength > 0f) { - entry.setValue(finalNewLength); - } - - // Reset the length to minimize bias in other variables, - // it's applied later - updateSkeletonBoneLength( + float leftDotProduct = getDotProductDiff( skeleton1, skeleton2, entry.getKey(), - originalLength + false, + slideLeft ); - } - } - } - - // Calculate average error over the epoch - float avgError = errorCount > 0 ? sumError / errorCount : -1f; - LogManager.info("[AutoBone] Epoch " + (epoch + 1) + " average error: " + avgError); - - if (epochCallback != null) { - epochCallback.accept(new Epoch(epoch + 1, numEpochs, avgError, configs)); - } - } - - float finalHeight = sumSelectConfigs(heightConfigs, configs, staticConfigs); - LogManager - .info("[AutoBone] Target height: " + targetHeight + " New height: " + finalHeight); - - return FastMath.abs(finalHeight - targetHeight); - } - - // The change in position of the ankle over time - protected float getSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { - float slideLeft = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position - .distance(skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position); - float slideRight = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position - .distance(skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position); - - // Divide by 4 to halve and average, it's halved because you want to - // approach a - // midpoint, not the other point - return (slideLeft + slideRight) / 4f; - } - - // The change in distance between both of the ankles over time - protected float getOffsetSlideErrorDeriv( - PoseFrameSkeleton skeleton1, - PoseFrameSkeleton skeleton2 - ) { - Vector3f leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position; - Vector3f rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position; - - Vector3f leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position; - Vector3f rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position; - - float slideDist1 = leftFoot1.distance(rightFoot1); - float slideDist2 = leftFoot2.distance(rightFoot2); - - float slideDist3 = leftFoot1.distance(rightFoot2); - float slideDist4 = leftFoot2.distance(rightFoot1); - - float dist1 = FastMath.abs(slideDist1 - slideDist2); - float dist2 = FastMath.abs(slideDist3 - slideDist4); - - float dist3 = FastMath.abs(slideDist1 - slideDist3); - float dist4 = FastMath.abs(slideDist1 - slideDist4); - - float dist5 = FastMath.abs(slideDist2 - slideDist3); - float dist6 = FastMath.abs(slideDist2 - slideDist4); - - // Divide by 12 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; - } - - // The offset between both feet at one instant and over time - protected float getOffsetErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { - float leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY(); - float rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY(); - - float leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY(); - float rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY(); - - float dist1 = FastMath.abs(leftFoot1 - rightFoot1); - float dist2 = FastMath.abs(leftFoot2 - rightFoot2); - float dist3 = FastMath.abs(leftFoot1 - rightFoot2); - float dist4 = FastMath.abs(leftFoot2 - rightFoot1); + float rightDotProduct = getDotProductDiff( + skeleton1, + skeleton2, + entry.getKey(), + true, + slideRight + ); - float dist5 = FastMath.abs(leftFoot1 - leftFoot2); - float dist6 = FastMath.abs(rightFoot1 - rightFoot2); + float dotLength = originalLength + * ((leftDotProduct + rightDotProduct) / 2f); - // Divide by 12 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; - } + // Scale by the ratio for smooth adjustment and more + // stable results + float curAdjustVal = (adjustVal * -dotLength) / totalLength; + float newLength = originalLength + curAdjustVal; - // The distance from average human proportions - protected float getProportionErrorDeriv(SkeletonConfig skeleton) { - float neckLength = skeleton.getConfig(SkeletonConfigValue.NECK); - float chestLength = skeleton.getConfig(SkeletonConfigValue.CHEST); - float torsoLength = skeleton.getConfig(SkeletonConfigValue.TORSO); - float legsLength = skeleton.getConfig(SkeletonConfigValue.LEGS_LENGTH); - float kneeHeight = skeleton.getConfig(SkeletonConfigValue.KNEE_HEIGHT); + // No small or negative numbers!!! Bad algorithm! + if (newLength < 0.01f) { + continue; + } - float chestTorso = FastMath.abs((chestLength / torsoLength) - chestTorsoRatio); - float legBody = FastMath.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio); - float kneeLeg = FastMath.abs((kneeHeight / legsLength) - kneeLegRatio); + // Apply new offset length + intermediateOffsets.put(entry.getKey(), newLength); + applyConfig(skeleton1.skeletonConfig, intermediateOffsets); + skeleton2.skeletonConfig.setConfigs(skeleton1.skeletonConfig); - if (legBody <= legBodyRatioRange) { - legBody = 0f; - } else { - legBody -= legBodyRatioRange; - } + // Update the skeleton poses for the new offset length + skeleton1.updatePose(); + skeleton2.updatePose(); - return (chestTorso + legBody + kneeLeg) / 3f; - } + float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight; + trainingStep.setCurrentHeight(newHeight); - // The distance of any points to the corresponding absolute position - protected float getPositionErrorDeriv( - PoseFrames frames, - int cursor, - PoseFrameSkeleton skeleton - ) { - float offset = 0f; - int offsetCount = 0; + float newErrorDeriv = getErrorDeriv(trainingStep); - List trackers = frames.getTrackers(); - for (PoseFrameTracker tracker : trackers) { - TrackerFrame trackerFrame = tracker.safeGetFrame(cursor); - if ( - trackerFrame == null - || !trackerFrame.hasData(TrackerFrameData.POSITION) - || trackerFrame.designation.trackerRole.isEmpty() - ) { - continue; - } + if (newErrorDeriv < errorDeriv) { + entry.setValue(newLength); + } - Vector3f nodePos = skeleton - .getComputedTracker(trackerFrame.designation.trackerRole.get()).position; - if (nodePos != null) { - offset += FastMath.abs(nodePos.distance(trackerFrame.position)); - offsetCount++; - } - } + // Reset the length to minimize bias in other variables, + // it's applied later + intermediateOffsets.put(entry.getKey(), originalLength); + applyConfig(skeleton1.skeletonConfig, intermediateOffsets); + skeleton2.skeletonConfig.setConfigs(skeleton1.skeletonConfig); + } - return offsetCount > 0 ? offset / offsetCount : 0f; - } + if (scaleEachStep) { + float stepHeight = sumSelectConfigs(heightOffsets, offsets); - // The difference between offset of absolute position and the corresponding - // point over time - protected float getPositionOffsetErrorDeriv( - PoseFrames frames, - int cursor1, - int cursor2, - PoseFrameSkeleton skeleton1, - PoseFrameSkeleton skeleton2 - ) { - float offset = 0f; - int offsetCount = 0; + 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; - List trackers = frames.getTrackers(); - for (PoseFrameTracker tracker : trackers) { - TrackerFrame trackerFrame1 = tracker.safeGetFrame(cursor1); - if (trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) { - continue; - } + float length = entry.getValue(); - TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2); - if ( - trackerFrame2 == null - || !trackerFrame2.hasData(TrackerFrameData.POSITION) - || trackerFrame1.designation.trackerRole.isEmpty() - ) { - continue; - } + // Multiply the diff by the length to height + // ratio + float adjVal = stepHeightDiff * (length / stepHeight); - Vector3f nodePos1 = skeleton1 - .getComputedTracker(trackerFrame1.designation.trackerRole.get()).position; - if (nodePos1 == null) { - continue; + // Scale the length to fit the target height + entry.setValue(Math.max(length + (adjVal / 2f), 0.01f)); + } + } + } + } } + // Calculate average error over the epoch + float avgError = errorCount > 0 ? sumError / errorCount : -1f; + LogManager.info("[AutoBone] Epoch " + (epoch + 1) + " average error: " + avgError); - if (trackerFrame2.designation.trackerRole.isEmpty()) { - continue; - } - Vector3f nodePos2 = skeleton2 - .getComputedTracker(trackerFrame2.designation.trackerRole.get()).position; - if (nodePos2 == null) { - continue; + applyConfig(legacyConfigs); + if (epochCallback != null) { + epochCallback.accept(new Epoch(epoch + 1, numEpochs, avgError, legacyConfigs)); } - - float dist1 = FastMath.abs(nodePos1.distance(trackerFrame1.position)); - float dist2 = FastMath.abs(nodePos2.distance(trackerFrame2.position)); - - offset += FastMath.abs(dist2 - dist1); - offsetCount++; } - return offsetCount > 0 ? offset / offsetCount : 0f; + float finalHeight = sumSelectConfigs(heightOffsets, offsets); + LogManager + .info( + "[AutoBone] Target height: " + + targetHeight + + " New height: " + + finalHeight + ); + + return new AutoBoneResults(finalHeight, targetHeight, legacyConfigs); } - protected float getErrorDeriv( - PoseFrames frames, - int cursor1, - int cursor2, - PoseFrameSkeleton skeleton1, - PoseFrameSkeleton skeleton2, - float heightChange, - float distScale - ) { + protected float getErrorDeriv(AutoBoneTrainingStep trainingStep) throws AutoBoneException { float totalError = 0f; float sumWeight = 0f; if (slideErrorFactor > 0f) { - // This is the main error function, this calculates the distance - // between the - // foot positions on both frames - totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor; + totalError += slideError.getStepError(trainingStep) * slideErrorFactor; sumWeight += slideErrorFactor; } if (offsetSlideErrorFactor > 0f) { - // This error function compares the distance between the feet on - // each frame and - // returns the offset between them - totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) - * distScale - * offsetSlideErrorFactor; + totalError += offsetSlideError.getStepError(trainingStep) * offsetSlideErrorFactor; sumWeight += offsetSlideErrorFactor; } - if (offsetErrorFactor > 0f) { - // This error function compares the height of each foot in each - // frame - totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * distScale * offsetErrorFactor; - sumWeight += offsetErrorFactor; + if (footHeightOffsetErrorFactor > 0f) { + totalError += footHeightOffsetError.getStepError(trainingStep) + * footHeightOffsetErrorFactor; + sumWeight += footHeightOffsetErrorFactor; } - if (proportionErrorFactor > 0f) { - // This error function compares the current values to general - // expected - // proportions to keep measurements in line - // Either skeleton will work fine, skeleton1 is used as a default - totalError += getProportionErrorDeriv(skeleton1.skeletonConfig) * proportionErrorFactor; - sumWeight += proportionErrorFactor; + if (bodyProportionErrorFactor > 0f) { + totalError += bodyProportionError.getStepError(trainingStep) + * bodyProportionErrorFactor; + sumWeight += bodyProportionErrorFactor; } if (heightErrorFactor > 0f) { - // This error function compares the height change to the actual - // measured height - // of the headset - totalError += FastMath.abs(heightChange) * heightErrorFactor; + totalError += heightError.getStepError(trainingStep) * heightErrorFactor; sumWeight += heightErrorFactor; } if (positionErrorFactor > 0f) { - // This error function compares the position of an assigned tracker - // with the - // position on the skeleton - totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) - + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) - * distScale - * positionErrorFactor; + totalError += positionError.getStepError(trainingStep) * positionErrorFactor; sumWeight += positionErrorFactor; } if (positionOffsetErrorFactor > 0f) { - // This error function compares the offset of the position of an - // assigned - // tracker with the position on the skeleton - totalError += getPositionOffsetErrorDeriv( - frames, - cursor1, - cursor2, - skeleton1, - skeleton2 - ) * distScale * positionOffsetErrorFactor; + totalError += positionOffsetError.getStepError(trainingStep) + * positionOffsetErrorFactor; sumWeight += positionOffsetErrorFactor; } return sumWeight > 0f ? totalError / sumWeight : 0f; } - protected void updateSkeletonBoneLength( - PoseFrameSkeleton skeleton1, - PoseFrameSkeleton skeleton2, - SkeletonConfigValue config, - float newLength - ) { - skeleton1.skeletonConfig.setConfig(config, newLength); - skeleton1.updatePoseAffectedByConfig(config); - - skeleton2.skeletonConfig.setConfig(config, newLength); - skeleton2.updatePoseAffectedByConfig(config); - } - public String getLengthsString() { final StringBuilder configInfo = new StringBuilder(); - this.configs.forEach((key, value) -> { + this.offsets.forEach((key, value) -> { if (configInfo.length() > 0) { configInfo.append(", "); } - configInfo.append(key.stringVal + ": " + StringUtils.prettyNumber(value * 100f, 2)); + configInfo.append(key.toString() + ": " + StringUtils.prettyNumber(value * 100f, 2)); }); return configInfo.toString(); @@ -927,4 +904,25 @@ public String toString() { return "Epoch: " + epoch + ", Epoch Error: " + epochError; } } + + public class AutoBoneResults { + + public final float finalHeight; + public final float targetHeight; + public final EnumMap configValues; + + public AutoBoneResults( + float finalHeight, + float targetHeight, + EnumMap configValues + ) { + this.finalHeight = finalHeight; + this.targetHeight = targetHeight; + this.configValues = configValues; + } + + public float getHeightDifference() { + return FastMath.abs(targetHeight - finalHeight); + } + } } diff --git a/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java b/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java index d2692ab6a4..1065106b46 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java +++ b/src/main/java/dev/slimevr/autobone/AutoBoneHandler.java @@ -8,9 +8,14 @@ import org.apache.commons.lang3.tuple.Pair; import dev.slimevr.VRServer; +import dev.slimevr.autobone.AutoBone.AutoBoneResults; +import dev.slimevr.autobone.errors.AutoBoneException; +import dev.slimevr.poserecorder.PoseFrameTracker; import dev.slimevr.poserecorder.PoseFrames; import dev.slimevr.poserecorder.PoseRecorder; +import dev.slimevr.vr.processor.skeleton.SkeletonConfig; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; +import dev.slimevr.vr.trackers.TrackerPosition; import io.eiren.util.StringUtils; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; @@ -82,7 +87,7 @@ public String getLengthsString() { return autoBone.getLengthsString(); } - private float processFrames(PoseFrames frames) { + private AutoBoneResults processFrames(PoseFrames frames) throws AutoBoneException { return autoBone .processFrames(frames, autoBone.calcInitError, autoBone.targetHeight, (epoch) -> { listeners.forEach(listener -> { @@ -298,39 +303,59 @@ private void processRecordingThread() { announceProcessStatus(AutoBoneProcessType.PROCESS, "Processing recording(s)..."); LogManager.info("[AutoBone] Processing frames..."); FastList heightPercentError = new FastList(frameRecordings.size()); + SkeletonConfig skeletonConfigBuffer = new SkeletonConfig(false); for (Pair recording : frameRecordings) { LogManager .info("[AutoBone] Processing frames from \"" + recording.getKey() + "\"..."); - heightPercentError.add(processFrames(recording.getValue())); + List trackers = recording.getValue().getTrackers(); + + StringBuilder trackerInfo = new StringBuilder(); + for (PoseFrameTracker tracker : trackers) { + if (tracker == null) + continue; + + TrackerPosition position = tracker + .getBodyPosition(); + if (position == null) + continue; + + if (trackerInfo.length() > 0) { + trackerInfo.append(", "); + } + + trackerInfo.append(position.designation); + } + + LogManager + .info( + "[AutoBone] (" + + trackers.size() + + " trackers) [" + + trackerInfo.toString() + + "]" + ); + + AutoBoneResults autoBoneResults = processFrames(recording.getValue()); + heightPercentError.add(autoBoneResults.getHeightDifference()); LogManager.info("[AutoBone] Done processing!"); // #region Stats/Values - Float neckLength = autoBone.getConfig(SkeletonConfigValue.NECK); - Float chestDistance = autoBone.getConfig(SkeletonConfigValue.CHEST); - Float torsoLength = autoBone.getConfig(SkeletonConfigValue.TORSO); - Float hipWidth = autoBone.getConfig(SkeletonConfigValue.HIPS_WIDTH); - Float legsLength = autoBone.getConfig(SkeletonConfigValue.LEGS_LENGTH); - Float kneeHeight = autoBone.getConfig(SkeletonConfigValue.KNEE_HEIGHT); - - float neckTorso = neckLength != null && torsoLength != null - ? neckLength / torsoLength - : 0f; - float chestTorso = chestDistance != null && torsoLength != null - ? chestDistance / torsoLength - : 0f; - float torsoWaist = hipWidth != null && torsoLength != null - ? hipWidth / torsoLength - : 0f; - float legTorso = legsLength != null && torsoLength != null - ? legsLength / torsoLength - : 0f; - float legBody = legsLength != null && torsoLength != null && neckLength != null - ? legsLength / (torsoLength + neckLength) - : 0f; - float kneeLeg = kneeHeight != null && legsLength != null - ? kneeHeight / legsLength - : 0f; + skeletonConfigBuffer.setConfigs(autoBoneResults.configValues, null); + + float neckLength = skeletonConfigBuffer.getConfig(SkeletonConfigValue.NECK); + float chestDistance = skeletonConfigBuffer.getConfig(SkeletonConfigValue.CHEST); + float torsoLength = skeletonConfigBuffer.getConfig(SkeletonConfigValue.TORSO); + float hipWidth = skeletonConfigBuffer.getConfig(SkeletonConfigValue.HIPS_WIDTH); + float legsLength = skeletonConfigBuffer.getConfig(SkeletonConfigValue.LEGS_LENGTH); + float kneeHeight = skeletonConfigBuffer.getConfig(SkeletonConfigValue.KNEE_HEIGHT); + + float neckTorso = neckLength / torsoLength; + float chestTorso = chestDistance / torsoLength; + float torsoWaist = hipWidth / torsoLength; + float legTorso = legsLength / torsoLength; + float legBody = legsLength / (torsoLength + neckLength); + float kneeLeg = kneeHeight / legsLength; LogManager .info( @@ -377,7 +402,7 @@ private void processRecordingThread() { // #endregion listeners.forEach(listener -> { - listener.onAutoBoneEnd(autoBone.configs); + listener.onAutoBoneEnd(autoBone.legacyConfigs); }); announceProcessStatus(AutoBoneProcessType.PROCESS, "Done processing!", true, true); @@ -395,7 +420,7 @@ private void processRecordingThread() { } public void applyValues() { - autoBone.applyConfig(); + autoBone.applyAndSaveConfig(); announceProcessStatus(AutoBoneProcessType.APPLY, "Adjusted values applied!", true, true); // TODO Update GUI values after applying? Is that needed here? } diff --git a/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java b/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java new file mode 100644 index 0000000000..debdc17991 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/AutoBoneTrainingStep.java @@ -0,0 +1,108 @@ +package dev.slimevr.autobone; + +import java.util.Map; + +import dev.slimevr.poserecorder.PoseFrameSkeleton; +import dev.slimevr.poserecorder.PoseFrames; +import dev.slimevr.vr.processor.skeleton.BoneType; + + +public class AutoBoneTrainingStep { + private int cursor1 = 0; + private int cursor2 = 0; + + private float currentHeight; + private final float targetHeight; + + private final PoseFrameSkeleton skeleton1; + private final PoseFrameSkeleton skeleton2; + + private final PoseFrames trainingFrames; + + private final Map intermediateOffsets; + + public AutoBoneTrainingStep( + int cursor1, + int cursor2, + float targetHeight, + PoseFrameSkeleton skeleton1, + PoseFrameSkeleton skeleton2, + PoseFrames trainingFrames, + Map intermediateOffsets + ) { + this.cursor1 = cursor1; + this.cursor2 = cursor2; + this.targetHeight = targetHeight; + this.skeleton1 = skeleton1; + this.skeleton2 = skeleton2; + this.trainingFrames = trainingFrames; + this.intermediateOffsets = intermediateOffsets; + } + + public AutoBoneTrainingStep( + float targetHeight, + PoseFrameSkeleton skeleton1, + PoseFrameSkeleton skeleton2, + PoseFrames trainingFrames, + Map intermediateOffsets + ) { + this.targetHeight = targetHeight; + this.skeleton1 = skeleton1; + this.skeleton2 = skeleton2; + 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 PoseFrameSkeleton getSkeleton1() { + return skeleton1; + } + + public PoseFrameSkeleton getSkeleton2() { + return skeleton2; + } + + public PoseFrames getTrainingFrames() { + return trainingFrames; + } + + public Map getIntermediateOffsets() { + return intermediateOffsets; + } + + public float getHeightOffset() { + return getTargetHeight() - getCurrentHeight(); + } +} diff --git a/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java b/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java new file mode 100644 index 0000000000..130dee79f9 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/AutoBoneException.java @@ -0,0 +1,28 @@ +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/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java b/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java new file mode 100644 index 0000000000..90a5c12ce9 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/BodyProportionError.java @@ -0,0 +1,53 @@ +package dev.slimevr.autobone.errors; + +import com.jme3.math.FastMath; + +import dev.slimevr.autobone.AutoBoneTrainingStep; +import dev.slimevr.vr.processor.skeleton.SkeletonConfig; +import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; + + +// The distance from average human proportions +public class BodyProportionError implements IAutoBoneError { + + // TODO hip tracker stuff... Hip tracker should be around 3 to 5 + // centimeters. Human average is probably 1.1235 (SD 0.07) + public float legBodyRatio = 1.1235f; + + // SD of 0.07, capture 68% within range + public float legBodyRatioRange = 0.07f; + + // kneeLegRatio seems to be around 0.54 to 0.6 after asking a few people in + // the SlimeVR discord. + public float kneeLegRatio = 0.55f; + + // kneeLegRatio seems to be around 0.55 to 0.64 after asking a few people in + // the SlimeVR discord. TODO : Chest should be a bit shorter (0.54?) if user + // has an additional hip tracker. + public float chestTorsoRatio = 0.57f; + + @Override + public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException { + return getBodyProportionError(trainingStep.getSkeleton1().skeletonConfig); + } + + public float getBodyProportionError(SkeletonConfig config) { + float neckLength = config.getConfig(SkeletonConfigValue.NECK); + float chestLength = config.getConfig(SkeletonConfigValue.CHEST); + float torsoLength = config.getConfig(SkeletonConfigValue.TORSO); + float legsLength = config.getConfig(SkeletonConfigValue.LEGS_LENGTH); + float kneeHeight = config.getConfig(SkeletonConfigValue.KNEE_HEIGHT); + + float chestTorso = FastMath.abs((chestLength / torsoLength) - chestTorsoRatio); + float legBody = FastMath.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio); + float kneeLeg = FastMath.abs((kneeHeight / legsLength) - kneeLegRatio); + + if (legBody <= legBodyRatioRange) { + legBody = 0f; + } else { + legBody -= legBodyRatioRange; + } + + return (chestTorso + legBody + kneeLeg) / 3f; + } +} diff --git a/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java b/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java new file mode 100644 index 0000000000..5d33a9cf4c --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/FootHeightOffsetError.java @@ -0,0 +1,54 @@ +package dev.slimevr.autobone.errors; + +import com.jme3.math.FastMath; + +import dev.slimevr.autobone.AutoBoneTrainingStep; +import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.trackers.ComputedTracker; +import dev.slimevr.vr.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.getSkeleton1(), trainingStep.getSkeleton2()); + } + + 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/src/main/java/dev/slimevr/autobone/errors/HeightError.java b/src/main/java/dev/slimevr/autobone/errors/HeightError.java new file mode 100644 index 0000000000..1caae4df24 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/HeightError.java @@ -0,0 +1,21 @@ +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/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java b/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java new file mode 100644 index 0000000000..d93634a61d --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/IAutoBoneError.java @@ -0,0 +1,8 @@ +package dev.slimevr.autobone.errors; + +import dev.slimevr.autobone.AutoBoneTrainingStep; + + +public interface IAutoBoneError { + public float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException; +} diff --git a/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java b/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java new file mode 100644 index 0000000000..2c72814a60 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/OffsetSlideError.java @@ -0,0 +1,61 @@ +package dev.slimevr.autobone.errors; + +import com.jme3.math.FastMath; +import com.jme3.math.Vector3f; + +import dev.slimevr.autobone.AutoBoneTrainingStep; +import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.trackers.ComputedTracker; +import dev.slimevr.vr.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.getSkeleton1(), trainingStep.getSkeleton2()); + } + + 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/src/main/java/dev/slimevr/autobone/errors/PositionError.java b/src/main/java/dev/slimevr/autobone/errors/PositionError.java new file mode 100644 index 0000000000..8f48fd4133 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/PositionError.java @@ -0,0 +1,61 @@ +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.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.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.getSkeleton1() + ) + + getPositionError( + trackers, + trainingStep.getCursor2(), + trainingStep.getSkeleton2() + )) + / 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.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/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java b/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java new file mode 100644 index 0000000000..0641911544 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.java @@ -0,0 +1,79 @@ +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.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.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.getSkeleton1(), + trainingStep.getSkeleton2() + ); + } + + 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.designation.trackerRole.isEmpty() + ) { + continue; + } + + TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2); + if ( + trackerFrame2 == null + || !trackerFrame2.hasData(TrackerFrameData.POSITION) + || 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/src/main/java/dev/slimevr/autobone/errors/SlideError.java b/src/main/java/dev/slimevr/autobone/errors/SlideError.java new file mode 100644 index 0000000000..cd58eee540 --- /dev/null +++ b/src/main/java/dev/slimevr/autobone/errors/SlideError.java @@ -0,0 +1,38 @@ +package dev.slimevr.autobone.errors; + +import dev.slimevr.autobone.AutoBoneTrainingStep; +import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.trackers.ComputedTracker; +import dev.slimevr.vr.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.getSkeleton1(), trainingStep.getSkeleton2()); + } + + 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/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java index 8374060993..bc267fcbb9 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java @@ -1347,6 +1347,61 @@ public void updateNodeOffset(BoneType nodeOffset, Vector3f offset) { } } + public TransformNode getNode(BoneType nodeOffset) { + if (nodeOffset == null) { + return null; + } + + switch (nodeOffset) { + case HEAD: + return headNode; + case NECK: + return neckNode; + case CHEST: + return chestNode; + case CHEST_TRACKER: + return trackerChestNode; + case WAIST: + return waistNode; + case HIP: + return hipNode; + case HIP_TRACKER: + return trackerWaistNode; + + case LEFT_HIP: + return leftHipNode; + case RIGHT_HIP: + return rightHipNode; + + case LEFT_UPPER_LEG: + return leftKneeNode; + case RIGHT_UPPER_LEG: + return rightKneeNode; + + case RIGHT_KNEE_TRACKER: + return trackerRightKneeNode; + case LEFT_KNEE_TRACKER: + return trackerLeftKneeNode; + + case LEFT_LOWER_LEG: + return leftAnkleNode; + case RIGHT_LOWER_LEG: + return rightAnkleNode; + + case LEFT_FOOT: + return leftFootNode; + case RIGHT_FOOT: + return rightFootNode; + + case LEFT_FOOT_TRACKER: + return trackerLeftFootNode; + case RIGHT_FOOT_TRACKER: + return trackerRightFootNode; + } + + return null; + } + public void updatePoseAffectedByConfig(SkeletonConfigValue config) { switch (config) { case HEAD: