Convert AutoBone to Kotlin and extend PoseFrame data functionality (#581)

This commit is contained in:
Butterscotch! authored and GitHub committed 2023-02-26 15:19:08 +03:00
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
}
}
}
@@ -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
}
@@ -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;
}
}
@@ -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
@@ -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);
}