Merge branch 'main' into test

# Conflicts:
#	src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java
This commit is contained in:
Eiren Rain committed 2022-01-20 17:54:56 +02:00
commit 2d1ffbc5b0
6 files changed
+104 -35

No files matched your search

+1 -1
View File
@@ -1,5 +1,5 @@
@echo off
echo Installing firewall rules...
echo Uninstalling firewall rules...
rem Discovery defauly port
netsh advfirewall firewall delete rule name="SlimeVR UDP 35903 incoming"
@@ -15,14 +15,13 @@ import dev.slimevr.poserecorder.PoseFrameTracker;
import dev.slimevr.poserecorder.PoseFrames;
import dev.slimevr.poserecorder.TrackerFrame;
import dev.slimevr.poserecorder.TrackerFrameData;
import dev.slimevr.vr.processor.HumanPoseProcessor;
import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
import dev.slimevr.vr.trackers.TrackerPosition;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.TrackerUtils;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.logging.LogManager;
import io.eiren.util.collections.FastList;
@@ -77,8 +76,6 @@ public class AutoBone {
protected final VRServer server;
protected SimpleSkeleton skeleton = null;
// This is filled by reloadConfigValues()
public final EnumMap<SkeletonConfigValue, Float> configs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
public final EnumMap<SkeletonConfigValue, Float> staticConfigs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
@@ -90,10 +87,7 @@ public class AutoBone {
public AutoBone(VRServer server) {
this.server = server;
reloadConfigValues();
server.addSkeletonUpdatedCallback(this::skeletonUpdated);
}
public void reloadConfigValues() {
@@ -139,17 +133,18 @@ public class AutoBone {
staticConfigs.put(SkeletonConfigValue.SKELETON_OFFSET, 0f);
}
@ThreadSafe
public void skeletonUpdated(HumanSkeleton newSkeleton) {
if(newSkeleton instanceof SimpleSkeleton) {
skeleton = (SimpleSkeleton) newSkeleton;
applyConfigToSkeleton(newSkeleton);
LogManager.log.info("[AutoBone] Received updated skeleton");
}
/**
* 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 HumanSkeleton getSkeleton() {
HumanPoseProcessor humanPoseProcessor = server != null ? server.humanPoseProcessor : null;
return humanPoseProcessor != null ? humanPoseProcessor.getSkeleton() : null;
}
public void applyConfig() {
if(!applyConfigToSkeleton(skeleton)) {
if(!applyConfigToSkeleton(getSkeleton())) {
// Unable to apply to skeleton, save directly
saveConfigs();
}
@@ -287,10 +282,14 @@ public class AutoBone {
// If target height isn't specified, auto-detect
if(targetHeight < 0f) {
// Get the current skeleton from the server
HumanSkeleton skeleton = getSkeleton();
if(skeleton != null) {
// If there is a skeleton available, calculate the target height from its configs
targetHeight = sumSelectConfigs(heightConfigs, skeleton.getSkeletonConfig());
LogManager.log.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 = getMaxHmdHeight(frames);
if(hmdHeight <= 0.50f) {
LogManager.log.warning("[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): " + hmdHeight);
@@ -303,12 +302,14 @@ public class AutoBone {
}
}
// Epoch loop, each epoch is one full iteration over the full dataset
for(int epoch = calcInitError ? -1 : 0; epoch < numEpochs; epoch++) {
float sumError = 0f;
int errorCount = 0;
float adjustRate = epoch >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f;
// Iterate over the frames using a cursor and an offset for comparing frames a certain number of frames apart
for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) {
for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) {
int frameCursor2 = frameCursor + cursorOffset;
@@ -559,42 +560,48 @@ public class AutoBone {
float sumWeight = 0f;
if(slideErrorFactor > 0f) {
// This is the main error function, this calculates the distance between the foot positions on both frames
totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor;
sumWeight += slideErrorFactor;
}
if(offsetSlideErrorFactor > 0f) {
// This error function compares the distance between the feet on each frame and returns the offset between them
totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) * distScale * offsetSlideErrorFactor;
sumWeight += offsetSlideErrorFactor;
}
if(offsetErrorFactor > 0f) {
// This error function compares the height of each foot in each frame
totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * distScale * offsetErrorFactor;
sumWeight += offsetErrorFactor;
}
if(proportionErrorFactor > 0f) {
// This error function compares the current values to general expected proportions to keep measurements in line
// Either skeleton will work fine, skeleton1 is used as a default
totalError += getProportionErrorDeriv(skeleton1.skeletonConfig) * proportionErrorFactor;
sumWeight += proportionErrorFactor;
}
if(heightErrorFactor > 0f) {
// This error function compares the height change to the actual measured height of the headset
totalError += FastMath.abs(heightChange) * heightErrorFactor;
sumWeight += heightErrorFactor;
}
if(positionErrorFactor > 0f) {
// This error function compares the position of an assigned tracker with the position on the skeleton
totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) * distScale * positionErrorFactor;
sumWeight += positionErrorFactor;
}
if(positionOffsetErrorFactor > 0f) {
// This error function compares the offset of the position of an assigned tracker with the position on the skeleton
totalError += getPositionOffsetErrorDeriv(frames, cursor1, cursor2, skeleton1, skeleton2) * distScale * positionOffsetErrorFactor;
sumWeight += positionOffsetErrorFactor;
}
// Minimize sliding, minimize foot height offset, minimize change in total height
return sumWeight > 0f ? totalError / sumWeight : 0f;
}
@@ -143,6 +143,9 @@ public class TrackersList extends EJBoxNoStretch {
JLabel adj;
JLabel adjYaw;
JLabel correction;
JLabel signalStrength;
JLabel rotQuat;
JLabel rotAdj;
@AWTThread
public TrackerPanel(Tracker t) {
@@ -216,6 +219,7 @@ public class TrackersList extends EJBoxNoStretch {
add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
if(realTracker instanceof IMUTracker) {
add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
add(new JLabel("Signal"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START));
}
row++;
if(t.hasRotation())
@@ -224,6 +228,7 @@ public class TrackersList extends EJBoxNoStretch {
add(position = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
if(realTracker instanceof IMUTracker) {
add(ping = new JLabel(""), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
add(signalStrength = new JLabel(""), c(4, row, 2, GridBagConstraints.FIRST_LINE_START));
}
if(realTracker instanceof TrackerWithTPS) {
add(tps = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
@@ -240,11 +245,19 @@ public class TrackersList extends EJBoxNoStretch {
row++;
add(new JLabel("Raw:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
add(raw = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
/*
if(realTracker instanceof IMUTracker) {
add(new JLabel("Quat:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
add(rotQuat = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
}
//*/
row++;
/*
if(realTracker instanceof IMUTracker) {
add(new JLabel("Raw mag:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
add(rawMag = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
add(new JLabel("Hash:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
add(new JLabel(String.format("0x%8x", realTracker.hashCode())), s(c(3, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
row++;
add(new JLabel("Cal:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
add(calibration = new JLabel("0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
@@ -253,6 +266,8 @@ public class TrackersList extends EJBoxNoStretch {
row++;
add(new JLabel("Correction:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
add(correction = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
add(new JLabel("RotAdj:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
add(rotAdj = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
row++;
}
//*/
@@ -297,7 +312,7 @@ public class TrackersList extends EJBoxNoStretch {
tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1));
}
if(realTracker instanceof TrackerWithBattery)
bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1));
bat.setText(String.format("%d%% (%sV)", Math.round(((TrackerWithBattery) realTracker).getBatteryLevel()), StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 2)));
if(t instanceof ReferenceAdjustedTracker) {
((ReferenceAdjustedTracker<Tracker>) t).attachmentFix.toAngles(angles);
if(adj != null)
@@ -313,6 +328,17 @@ public class TrackersList extends EJBoxNoStretch {
if(realTracker instanceof IMUTracker) {
if(ping != null)
ping.setText(String.valueOf(((IMUTracker) realTracker).ping));
if (signalStrength != null) {
int signal = ((IMUTracker) realTracker).signalStrength;
if (signal == -1) {
signalStrength.setText("N/A");
} else {
// -40 dBm is excellent, -95 dBm is very poor
int percentage = (signal - -95) * (100 - 0) / (-40 - -95) + 0;
percentage = Math.max(Math.min(percentage, 100), 0);
signalStrength.setText(String.valueOf(percentage) + "% " + "(" + String.valueOf(signal) + " dBm" + ")");
}
}
}
realTracker.getRotation(q);
q.toAngles(angles);
@@ -320,21 +346,34 @@ public class TrackersList extends EJBoxNoStretch {
+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
if(realTracker instanceof IMUTracker) {
((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles);
IMUTracker imu = (IMUTracker) realTracker;
imu.rotMagQuaternion.toAngles(angles);
if(rawMag != null)
rawMag.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
if(calibration != null)
calibration.setText(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus);
calibration.setText(imu.calibrationStatus + " / " + imu.magCalibrationStatus);
if(magAccuracy != null)
magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
((IMUTracker) realTracker).getCorrection(q);
magAccuracy.setText(StringUtils.prettyNumber(imu.magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
imu.getCorrection(q);
q.toAngles(angles);
if(correction != null)
correction.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
if(rotQuat != null) {
imu.rotQuaternion.toAngles(angles);
rotQuat.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
}
if(rotAdj != null) {
imu.rotAdjust.toAngles(angles);
rotAdj.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
}
}
}
}
@@ -7,13 +7,13 @@ public enum SkeletonConfigValue {
HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HEAD}),
NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.NECK}),
TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}),
CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}),
TORSO("Torso", "torsoLength", "Torso length", 0.6f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}),
CHEST("Chest", "chestDistance", "Chest distance", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}),
WAIST("Waist", "waistDistance", "Waist distance", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP}),
HIP_OFFSET("Hip offset", "hipOffset", "Hip offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HIP_TRACKER}),
HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}),
LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}),
KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}),
HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.28f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}),
LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.88f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}),
KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.44f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}),
FOOT_LENGTH("Foot length", "footLength", "Foot length", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.FOOT}),
FOOT_OFFSET("Foot offset", "footOffset", "Foot offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.ANKLE}),
SKELETON_OFFSET("Skeleton offset", "skeletonOffset", "Skeleton offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST_TRACKER, SkeletonNodeOffset.HIP_TRACKER, SkeletonNodeOffset.KNEE_TRACKER, SkeletonNodeOffset.FOOT_TRACKER}),
@@ -4,19 +4,18 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import io.eiren.math.FloatMath;
import io.eiren.util.BufferedTimer;
public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG;
public final Vector3f gyroVector = new Vector3f();
public final Vector3f accelVector = new Vector3f();
//public final Vector3f gyroVector = new Vector3f();
//public final Vector3f accelVector = new Vector3f();
public final Vector3f magVector = new Vector3f();
public final Quaternion rotQuaternion = new Quaternion();
public final Quaternion rotMagQuaternion = new Quaternion();
protected final Quaternion rotAdjust = new Quaternion();
public final Quaternion rotAdjust = new Quaternion();
protected final Quaternion correction = new Quaternion();
protected TrackerMountingRotation mounting = null;
protected TrackerStatus status = TrackerStatus.OK;
@@ -27,6 +26,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
protected final TrackersUDPServer server;
protected float confidence = 0;
protected float batteryVoltage = 0;
protected float batteryLevel = 0;
public int calibrationStatus = 0;
public int magCalibrationStatus = 0;
public float magnetometerAccuracy = 0;
@@ -35,6 +35,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
protected BufferedTimer timer = new BufferedTimer(1f);
public int ping = -1;
public int signalStrength = -1;
public TrackerPosition bodyPosition = null;
@@ -147,7 +148,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
@Override
public float getBatteryLevel() {
return FloatMath.mapValue(getBatteryVoltage(), 3.6f, 4.2f, 0f, 1f);
return batteryLevel;
}
@Override
@@ -155,6 +156,10 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
return batteryVoltage;
}
public void setBatteryLevel(float level) {
this.batteryLevel = level;
}
public void setBatteryVoltage(float voltage) {
this.batteryVoltage = voltage;
}
@@ -109,6 +109,8 @@ public class TrackersUDPServer extends Thread {
}
}
if(firmware.length() == 0) {
// TODO Owo track can report firmware
// Will be not important after refactoring, but need not forget
firmware.append("owoTrack");
tracker.isOwoTrack = true;
}
@@ -332,11 +334,16 @@ public class TrackersUDPServer extends Thread {
if(connection == null)
break;
bb.getLong();
float voltage = bb.getFloat();
float level = bb.getFloat() * 100; // Use default level if recieved 0
if(connection.sensors.size() > 0) {
Collection<IMUTracker> trackers = connection.sensors.values();
Iterator<IMUTracker> iterator = trackers.iterator();
while(iterator.hasNext())
iterator.next().setBatteryVoltage(bb.getFloat());
while(iterator.hasNext()) {
IMUTracker tr = iterator.next();
tr.setBatteryVoltage(voltage);
tr.setBatteryLevel(bb.getFloat() * 100);
}
}
break;
case 13: // PACKET_TAP
@@ -381,6 +388,17 @@ public class TrackersUDPServer extends Thread {
socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus);
break;
case 19:
if(connection == null)
break;
bb.getLong();
sensorId = bb.get() & 0xFF;
tracker = connection.sensors.get(sensorId);
if(tracker == null)
break;
int signalStrength = bb.get();
tracker.signalStrength = signalStrength;
break;
default:
System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
break;