mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Convert AutoBone to Kotlin and extend PoseFrame data functionality (#581)
This commit is contained in:
1 parent
ea9dc0b0ec
commit
8efbb90ded
44 files changed
+1993
-2268
No files matched your search
@@ -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<BoneType, Float> offsets = new EnumMap<>(
|
||||
BoneType.class
|
||||
);
|
||||
|
||||
public final FastList<BoneType> 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<BoneType> 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<SkeletonConfigOffsets> legacyHeightConfigs = new FastList<>(
|
||||
new SkeletonConfigOffsets[] {
|
||||
SkeletonConfigOffsets.NECK,
|
||||
SkeletonConfigOffsets.CHEST,
|
||||
SkeletonConfigOffsets.WAIST,
|
||||
SkeletonConfigOffsets.HIP,
|
||||
SkeletonConfigOffsets.UPPER_LEG,
|
||||
SkeletonConfigOffsets.LOWER_LEG,
|
||||
}
|
||||
);
|
||||
|
||||
public final EnumMap<SkeletonConfigOffsets, Float> 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<SkeletonConfigOffsets, Float> 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<PoseFrameTracker> trackers) {
|
||||
// Remove all previous values
|
||||
offsets.clear();
|
||||
|
||||
// Get current or default skeleton configs
|
||||
HumanPoseManager skeleton = getHumanPoseManager();
|
||||
Function<SkeletonConfigOffsets, Float> 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<SkeletonConfigOffsets, Float> configConsumer,
|
||||
Map<BoneType, Float> 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<SkeletonConfigOffsets, Float> configConsumer) {
|
||||
return applyConfig(configConsumer, offsets);
|
||||
}
|
||||
|
||||
public boolean applyConfig(
|
||||
Map<SkeletonConfigOffsets, Float> skeletonConfig,
|
||||
Map<BoneType, Float> offsets
|
||||
) {
|
||||
if (skeletonConfig == null) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return applyConfig(skeletonConfig::put, offsets);
|
||||
}
|
||||
|
||||
public boolean applyConfig(Map<SkeletonConfigOffsets, Float> skeletonConfig) {
|
||||
return applyConfig(skeletonConfig, offsets);
|
||||
}
|
||||
|
||||
public boolean applyConfig(
|
||||
HumanPoseManager humanPoseManager,
|
||||
Map<BoneType, Float> 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 <T> float sumSelectConfigs(
|
||||
List<T> selection,
|
||||
Function<T, Float> configs
|
||||
) {
|
||||
float sum = 0f;
|
||||
|
||||
for (T config : selection) {
|
||||
Float length = configs.apply(config);
|
||||
if (length != null) {
|
||||
sum += length;
|
||||
}
|
||||
}
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
public <T> float sumSelectConfigs(
|
||||
List<T> selection,
|
||||
Map<T, Float> configs
|
||||
) {
|
||||
return sumSelectConfigs(selection, configs::get);
|
||||
}
|
||||
|
||||
public float sumSelectConfigs(
|
||||
List<SkeletonConfigOffsets> selection,
|
||||
HumanPoseManager humanPoseManager
|
||||
) {
|
||||
return sumSelectConfigs(selection, humanPoseManager::getOffset);
|
||||
}
|
||||
|
||||
public float getLengthSum(Map<BoneType, Float> configs) {
|
||||
return getLengthSum(configs, null);
|
||||
}
|
||||
|
||||
public float getLengthSum(
|
||||
Map<BoneType, Float> configs,
|
||||
Map<BoneType, Float> configsAlt
|
||||
) {
|
||||
float length = 0f;
|
||||
|
||||
if (configsAlt != null) {
|
||||
for (Entry<BoneType, Float> 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<Epoch> epochCallback)
|
||||
throws AutoBoneException {
|
||||
return processFrames(frames, -1f, epochCallback);
|
||||
}
|
||||
|
||||
public AutoBoneResults processFrames(
|
||||
PoseFrames frames,
|
||||
float targetHeight,
|
||||
Consumer<Epoch> epochCallback
|
||||
) throws AutoBoneException {
|
||||
return processFrames(frames, true, targetHeight, epochCallback);
|
||||
}
|
||||
|
||||
public AutoBoneResults processFrames(
|
||||
PoseFrames frames,
|
||||
boolean calcInitError,
|
||||
float targetHeight,
|
||||
Consumer<Epoch> epochCallback
|
||||
) throws AutoBoneException {
|
||||
final int frameCount = frames.getMaxFrameCount();
|
||||
|
||||
final PoseFrames frames1 = new PoseFrames(frames);
|
||||
final PoseFrames frames2 = new PoseFrames(frames);
|
||||
|
||||
List<PoseFrameTracker> trackers1 = frames1.getTrackers();
|
||||
List<PoseFrameTracker> 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<BoneType, Float> 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<BoneType, Float> 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<BoneType, Float> 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<Pair<String, PoseFrames>> loadRecordings() {
|
||||
List<Pair<String, PoseFrames>> 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<SkeletonConfigOffsets, Float> configValues;
|
||||
|
||||
public Epoch(
|
||||
int epoch,
|
||||
int totalEpochs,
|
||||
StatsCalculator epochError,
|
||||
EnumMap<SkeletonConfigOffsets, Float> 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<SkeletonConfigOffsets, Float> configValues;
|
||||
|
||||
public AutoBoneResults(
|
||||
float finalHeight,
|
||||
float targetHeight,
|
||||
StatsCalculator epochError,
|
||||
EnumMap<SkeletonConfigOffsets, Float> configValues
|
||||
) {
|
||||
this.finalHeight = finalHeight;
|
||||
this.targetHeight = targetHeight;
|
||||
this.epochError = epochError;
|
||||
this.configValues = configValues;
|
||||
}
|
||||
|
||||
public float getHeightDifference() {
|
||||
return FastMath.abs(targetHeight - finalHeight);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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, Float>(
|
||||
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, Float>(
|
||||
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<SkeletonConfigOffsets?, Float>,
|
||||
): 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<PoseFrameTracker?>? = null) {
|
||||
// Remove all previous values
|
||||
offsets.clear()
|
||||
|
||||
// Get current or default skeleton configs
|
||||
val skeleton = humanPoseManager
|
||||
val getOffset: Function<SkeletonConfigOffsets?, Float> =
|
||||
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<SkeletonConfigOffsets, Float>,
|
||||
offsets: Map<BoneType, Float> = 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<SkeletonConfigOffsets, Float>,
|
||||
offsets: Map<BoneType, Float> = this.offsets,
|
||||
): Boolean {
|
||||
return applyConfig({ key: SkeletonConfigOffsets, value: Float -> skeletonConfig[key] = value }, offsets)
|
||||
}
|
||||
|
||||
fun applyConfig(
|
||||
humanPoseManager: HumanPoseManager,
|
||||
offsets: Map<BoneType, Float> = 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 <T> sumSelectConfigs(
|
||||
selection: List<T>,
|
||||
configs: Function<T, Float?>,
|
||||
): Float {
|
||||
var sum = 0f
|
||||
for (config in selection) {
|
||||
val length = configs.apply(config)
|
||||
if (length != null) {
|
||||
sum += length
|
||||
}
|
||||
}
|
||||
return sum
|
||||
}
|
||||
|
||||
fun <T> sumSelectConfigs(
|
||||
selection: List<T>,
|
||||
configs: Map<T, Float>,
|
||||
): Float {
|
||||
return sumSelectConfigs(selection) { key: T -> configs[key] }
|
||||
}
|
||||
|
||||
fun sumSelectConfigs(
|
||||
selection: List<SkeletonConfigOffsets>,
|
||||
humanPoseManager: HumanPoseManager,
|
||||
): Float {
|
||||
return sumSelectConfigs(selection) { key: SkeletonConfigOffsets? -> humanPoseManager.getOffset(key) }
|
||||
}
|
||||
|
||||
fun getLengthSum(configs: Map<BoneType, Float>): Float {
|
||||
return getLengthSum(configs, null)
|
||||
}
|
||||
|
||||
fun getLengthSum(
|
||||
configs: Map<BoneType, Float>,
|
||||
configsAlt: Map<BoneType, Float>?,
|
||||
): 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<Epoch?>?,
|
||||
): 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<Pair<String, PoseFrames>> {
|
||||
val recordings = FastList<Pair<String, PoseFrames>>()
|
||||
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<SkeletonConfigOffsets, Float>,
|
||||
) {
|
||||
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<SkeletonConfigOffsets, Float>,
|
||||
) {
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<AutoBoneListener> 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<PoseFrames> 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<PoseFrames> 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<Pair<String, PoseFrames>> frameRecordings = autoBone.loadRecordings();
|
||||
|
||||
if (!frameRecordings.isEmpty()) {
|
||||
LogManager.info("[AutoBone] Done loading frames!");
|
||||
} else {
|
||||
Future<PoseFrames> 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("<Recording>", 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<String, PoseFrames> recording : frameRecordings) {
|
||||
LogManager
|
||||
.info("[AutoBone] Processing frames from \"" + recording.getKey() + "\"...");
|
||||
|
||||
List<PoseFrameTracker> 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?
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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<AutoBoneListener>()
|
||||
|
||||
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("<Recording>", 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
|
||||
)
|
||||
}
|
||||
}
|
||||
@@ -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<SkeletonConfigOffsets, Float> configValues);
|
||||
}
|
||||
@@ -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<SkeletonConfigOffsets, Float>)
|
||||
}
|
||||
@@ -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<Integer, AutoBoneProcessType> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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 }
|
||||
@@ -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<BoneType, Float> intermediateOffsets;
|
||||
|
||||
public AutoBoneTrainingStep(
|
||||
int cursor1,
|
||||
int cursor2,
|
||||
float targetHeight,
|
||||
HumanPoseManager humanPoseManager1,
|
||||
HumanPoseManager humanPoseManager2,
|
||||
PoseFrames trainingFrames,
|
||||
Map<BoneType, Float> 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<BoneType, Float> 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<BoneType, Float> getIntermediateOffsets() {
|
||||
return intermediateOffsets;
|
||||
}
|
||||
|
||||
public float getHeightOffset() {
|
||||
return getTargetHeight() - getCurrentHeight();
|
||||
}
|
||||
}
|
||||
@@ -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<BoneType, Float>,
|
||||
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
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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<ProportionLimiter>(
|
||||
// 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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
@@ -1,8 +0,0 @@
|
||||
package dev.slimevr.autobone.errors;
|
||||
|
||||
import dev.slimevr.autobone.AutoBoneTrainingStep;
|
||||
|
||||
|
||||
public interface IAutoBoneError {
|
||||
float getStepError(AutoBoneTrainingStep trainingStep) throws AutoBoneException;
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
package dev.slimevr.autobone.errors
|
||||
|
||||
import dev.slimevr.autobone.AutoBoneTrainingStep
|
||||
|
||||
interface IAutoBoneError {
|
||||
@Throws(AutoBoneException::class)
|
||||
fun getStepError(trainingStep: AutoBoneTrainingStep): Float
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<PoseFrameTracker> 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<PoseFrameTracker> 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;
|
||||
}
|
||||
}
|
||||
@@ -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<PoseFrameTracker>,
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<PoseFrameTracker> trackers = trainingStep.getTrainingFrames().getTrackers();
|
||||
return getPositionOffsetError(
|
||||
trackers,
|
||||
trainingStep.getCursor1(),
|
||||
trainingStep.getCursor2(),
|
||||
trainingStep.getHumanPoseManager1().getSkeleton(),
|
||||
trainingStep.getHumanPoseManager2().getSkeleton()
|
||||
);
|
||||
}
|
||||
|
||||
public float getPositionOffsetError(
|
||||
List<PoseFrameTracker> 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;
|
||||
}
|
||||
}
|
||||
@@ -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<PoseFrameTracker>,
|
||||
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
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
-36
@@ -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<HumanPoseManager, Float> 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<HumanPoseManager, Float> 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;
|
||||
}
|
||||
}
|
||||
@@ -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<HumanPoseManager, Float>,
|
||||
) : ProportionLimiter {
|
||||
override fun getProportionError(humanPoseManager: HumanPoseManager, height: Float): Float {
|
||||
val boneLength = boneLengthFunction.apply(humanPoseManager)
|
||||
return targetRatio - boneLength / height
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
-79
@@ -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<HumanPoseManager, Float> 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<HumanPoseManager, Float> 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;
|
||||
}
|
||||
}
|
||||
+65
@@ -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<HumanPoseManager, Float>,
|
||||
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<HumanPoseManager, Float>,
|
||||
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
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
|
||||
@@ -145,9 +145,8 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
|
||||
@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<TrackerFrame> {
|
||||
@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<TrackerFrame> {
|
||||
@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<TrackerFrame> {
|
||||
|
||||
@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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -298,7 +298,7 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
|
||||
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<RpcMessageHeader>
|
||||
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<RpcMessageHeader>
|
||||
.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<SkeletonConfigOffsets, Float> skeletonConfig : epoch.configValues
|
||||
Entry<SkeletonConfigOffsets, Float> skeletonConfig : epoch
|
||||
.getConfigValues()
|
||||
.entrySet()
|
||||
) {
|
||||
skeletonPartOffsets[i++] = SkeletonPart
|
||||
@@ -385,9 +386,9 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
|
||||
int update = AutoBoneEpochResponse
|
||||
.createAutoBoneEpochResponse(
|
||||
fbb,
|
||||
epoch.epoch,
|
||||
epoch.totalEpochs,
|
||||
epoch.epochError.getMean(),
|
||||
epoch.getEpoch(),
|
||||
epoch.getTotalEpochs(),
|
||||
epoch.getEpochError().getMean(),
|
||||
skeletonPartsOffset
|
||||
);
|
||||
int outbound = this
|
||||
|
||||
+11
-7
@@ -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);
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user