github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 160 Packages Projects Releases Wiki Activity

Most trackers now have TPS

Browse Source 
Track trackers disconnect
Display raw magnetometer and accel readings for IMU trackers
Minor fixes
This commit is contained in:
Eiren Rain
2021-02-04 02:58:16 +03:00
parent 4d35be0ce0
commit 20f450f496
11 changed files with 165 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
src/main/java/io/eiren/gui/TrackersList.java
View File

@@ -25,6 +25,7 @@ import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.IMUTracker;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerConfig;
import io.eiren.vr.trackers.TrackerWithTPS;
public class TrackersList extends EJBag {
@@ -94,7 +95,7 @@ public class TrackersList extends EJBag {
});
}}, c(11, n, 2));
}
n++;
n += tr.getSize();
}
gui.refresh();
}
@@ -110,7 +111,10 @@ public class TrackersList extends EJBag {
@ThreadSafe
public void newTrackerAdded(Tracker t) {
java.awt.EventQueue.invokeLater(() -> {
trackers.add(new TrackerRow(t));
if(t instanceof IMUTracker)
trackers.add(new IMUTrackerRow((IMUTracker) t));
else
trackers.add(new TrackerRow(t));
build();
});
}
@@ -143,7 +147,7 @@ public class TrackersList extends EJBag {
add(a2 = new JLabel("0"), c(6, n, 2, GridBagConstraints.FIRST_LINE_START));
add(a3 = new JLabel("0"), c(7, n, 2, GridBagConstraints.FIRST_LINE_START));
add(status = new JLabel(t.getStatus().toString()), c(8, n, 2, GridBagConstraints.FIRST_LINE_START));
if(t instanceof IMUTracker) {
if(t instanceof TrackerWithTPS) {
add(tps = new JLabel("0"), c(9, n, 2, GridBagConstraints.FIRST_LINE_START));
} else {
add(new JLabel(""), c(9, n, 2, GridBagConstraints.FIRST_LINE_START));
@@ -168,11 +172,65 @@ public class TrackersList extends EJBag {
a3.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
status.setText(t.getStatus().toString());
if(t instanceof IMUTracker) {
tps.setText(StringUtils.prettyNumber(((IMUTracker) t).getTPS(), 1));
if(t instanceof TrackerWithTPS) {
tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) t).getTPS(), 1));
}
conf.setText(StringUtils.prettyNumber(t.getConfidenceLevel(), 1));
}
public int getSize() {
return 1;
}
}
private class IMUTrackerRow extends TrackerRow {
IMUTracker it;
JLabel mx;
JLabel my;
JLabel mz;
JLabel ax;
JLabel ay;
JLabel az;
public IMUTrackerRow(IMUTracker t) {
super(t);
it = t;
}
@Override
public TrackerRow build(int n) {
super.build(n);
n++;
add(new JLabel("Mag / Accel:"), c(0, n, 2, GridBagConstraints.FIRST_LINE_END));
add(mx = new JLabel("0"), c(2, n, 2, GridBagConstraints.FIRST_LINE_START));
add(my = new JLabel("0"), c(3, n, 2, GridBagConstraints.FIRST_LINE_START));
add(mz = new JLabel("0"), c(4, n, 2, GridBagConstraints.FIRST_LINE_START));
add(ax = new JLabel("0"), c(5, n, 2, GridBagConstraints.FIRST_LINE_START));
add(ay = new JLabel("0"), c(6, n, 2, GridBagConstraints.FIRST_LINE_START));
add(az = new JLabel("0"), c(7, n, 2, GridBagConstraints.FIRST_LINE_START));
return this;
}
@Override
public void update() {
super.update();
q.lookAt(it.magVector, Vector3f.UNIT_Y);
q.toAngles(angles);
mx.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0));
my.setText(StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0));
mz.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
q.lookAt(it.accelVector, Vector3f.UNIT_Y);
q.toAngles(angles);
ax.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0));
ay.setText(StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0));
az.setText(StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
}
@Override
public int getSize() {
return super.getSize() + 1;
}
}
private static int getTrackerSort(Tracker t) {

2
src/main/java/io/eiren/gui/VRServerGUI.java
View File

@@ -79,7 +79,7 @@ public class VRServerGUI extends JFrame {
add(new EJBox(PAGE_AXIS) {{
setAlignmentY(TOP_ALIGNMENT);
add(new JLabel("Sekelton"));
add(new JLabel("Skeleton"));
add(skeletonList);
add(Box.createVerticalGlue());
}});

1
src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java
View File

@@ -94,6 +94,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
hmd.position.set((float) x, (float) y, (float) z);
hmd.rotation.set((float) qx, (float) qy, (float) qz, (float) qw);
hmd.dataTick();
} catch(NumberFormatException e) {
e.printStackTrace();
}

15
src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java
View File

@@ -1,13 +1,26 @@
package io.eiren.vr.processor;
import io.eiren.util.BufferedTimer;
import io.eiren.vr.trackers.ComputedTracker;
import io.eiren.vr.trackers.TrackerWithTPS;
public class ComputedHumanPoseTracker extends ComputedTracker {
public class ComputedHumanPoseTracker extends ComputedTracker implements TrackerWithTPS {
public final ComputedHumanPoseTrackerPosition skeletonPosition;
protected BufferedTimer timer = new BufferedTimer(1f);
public ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition skeletonPosition) {
super("human://" + skeletonPosition.name());
this.skeletonPosition = skeletonPosition;
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}
}

7
src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java
View File

@@ -6,7 +6,6 @@ import java.util.Map;
import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import io.eiren.math.FloatMath;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerStatus;
@@ -150,11 +149,13 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
hipBuf.toAngles(hipAngles);
kneeBuf.toAngles(kneeAngles);
// TODO
hipConfidense = Math.abs(1 - hipConfidense);
kneeConfidense = Math.abs(1 - kneeConfidense);
float confidenseDiff = hipConfidense < kneeConfidense ? 1 : 0;//FloatMath.equalsToZero(hipConfidense) ? 1.0f : FastMath.clamp(0.5f * (kneeConfidense / hipConfidense), 0f, 1f);
hipAngles[1] = kneeAngles[1] = interpolateRadians(confidenseDiff, kneeAngles[1], hipAngles[1]);
hipAngles[1] = kneeAngles[1] = interpolateRadians(0.5f, kneeAngles[1], hipAngles[1]);
//hipAngles[2] = kneeAngles[2] = interpolateRadians(kneeLerpFactor, kneeAngles[2], hipAngles[2]);
hipBuf.fromAngles(hipAngles[0], hipAngles[1], hipAngles[2]);
@@ -184,8 +185,10 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
computedLeftAnkleTracker.position.set(leftAnkleNode.worldTransform.getTranslation());
computedLeftAnkleTracker.rotation.set(leftAnkleNode.worldTransform.getRotation());
computedLeftAnkleTracker.dataTick();
computedRightAnkleTracker.position.set(rightAnkleNode.worldTransform.getTranslation());
computedRightAnkleTracker.rotation.set(rightAnkleNode.worldTransform.getRotation());
computedRightAnkleTracker.dataTick();
}
}

9
src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java
View File

@@ -144,15 +144,16 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
// Pelvic bone doesn't tilt when humans tilt, unless they really try.
// Can't calculate tilt without additional sensors, so just remove it
// completely.
vBuf.set(0, 0, 1);
qBuf.multLocal(vBuf);
vBuf.multLocal(1, 0, 1); // Keep only yaw / Don't normalize, it's done by lookAt()
qBuf.lookAt(vBuf, Vector3f.UNIT_Y);
//vBuf.set(0, 0, 1);
//qBuf.multLocal(vBuf);
//vBuf.multLocal(1, 0, 1); // Keep only yaw / Don't normalize, it's done by lookAt()
//qBuf.lookAt(vBuf, Vector3f.UNIT_Y);
waistNode.localTransform.setRotation(qBuf);
}
protected void updateComputedTrackers() {
computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation());
computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
computedWaistTracker.dataTick();
}
}

16
src/main/java/io/eiren/vr/trackers/AdjustedTracker.java
View File

@@ -11,7 +11,7 @@ public class AdjustedTracker implements Tracker {
private final Quaternion smoothedQuaternion = new Quaternion();
private float[] angles = new float[3];
protected float[] lastAngles = new float[3];
public float smooth = 3 * FastMath.DEG_TO_RAD;
public float smooth = 0 * FastMath.DEG_TO_RAD;
protected float confidenceMultiplier = 1.0f;
@@ -51,12 +51,14 @@ public class AdjustedTracker implements Tracker {
@Override
public boolean getRotation(Quaternion store) {
tracker.getRotation(store);
store.toAngles(angles);
if(Math.abs(angles[0] - lastAngles[0]) > smooth || Math.abs(angles[1] - lastAngles[1]) > smooth || Math.abs(angles[2] - lastAngles[2]) > smooth) {
smoothedQuaternion.set(store);
store.toAngles(lastAngles);
} else {
store.set(smoothedQuaternion);
if(smooth > 0) {
store.toAngles(angles);
if(Math.abs(angles[0] - lastAngles[0]) > smooth || Math.abs(angles[1] - lastAngles[1]) > smooth || Math.abs(angles[2] - lastAngles[2]) > smooth) {
smoothedQuaternion.set(store);
store.toAngles(lastAngles);
} else {
store.set(smoothedQuaternion);
}
}
adjustment.mult(store, store);

15
src/main/java/io/eiren/vr/trackers/HMDTracker.java
View File

@@ -1,9 +1,22 @@
package io.eiren.vr.trackers;
public class HMDTracker extends ComputedTracker {
import io.eiren.util.BufferedTimer;
public class HMDTracker extends ComputedTracker implements TrackerWithTPS {
protected BufferedTimer timer = new BufferedTimer(1f);
public HMDTracker(String name) {
super(name);
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}
}

4
src/main/java/io/eiren/vr/trackers/IMUTracker.java
View File

@@ -8,7 +8,7 @@ import com.jme3.math.Vector3f;
import io.eiren.util.BufferedTimer;
public class IMUTracker implements Tracker, CalibratingTracker {
public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS {
public final Vector3f gyroVector = new Vector3f();
public final Vector3f accelVector = new Vector3f();
@@ -67,10 +67,12 @@ public class IMUTracker implements Tracker, CalibratingTracker {
server.sendCalibrationCommand(this, calibrationDataConsumer);
}
@Override
public float getTPS() {
return timer.getAverageFPS();
}
@Override
public void dataTick() {
timer.update();
}

8
src/main/java/io/eiren/vr/trackers/TrackerWithTPS.java Normal file
View File

@@ -0,0 +1,8 @@
package io.eiren.vr.trackers;
public interface TrackerWithTPS {
public float getTPS();
public void dataTick();
}

53
src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java
View File

@@ -149,6 +149,12 @@ public class TrackersUDPServer extends Thread {
double[] accelData = new double[sensor.rawCalibrationData.size() * 3];
double[] magData = new double[sensor.rawCalibrationData.size() * 3];
double magMinX = Double.MAX_VALUE;
double magMinY = Double.MAX_VALUE;
double magMinZ = Double.MAX_VALUE;
double magMaxX = Double.MIN_VALUE;
double magMaxY = Double.MIN_VALUE;
double magMaxZ = Double.MIN_VALUE;
for(int i = 0; i < sensor.rawCalibrationData.size(); ++i) {
double[] line = sensor.rawCalibrationData.get(i);
accelData[i * 3 + 0] = line[0];
@@ -157,15 +163,21 @@ public class TrackersUDPServer extends Thread {
magData[i * 3 + 0] = line[3];
magData[i * 3 + 1] = line[4];
magData[i * 3 + 2] = line[5];
magMinX = Math.min(magMinX, line[3]);
magMinY = Math.min(magMinY, line[4]);
magMinZ = Math.min(magMinZ, line[5]);
magMaxX = Math.max(magMaxX, line[3]);
magMaxY = Math.max(magMaxY, line[4]);
magMaxZ = Math.max(magMaxZ, line[5]);
}
double accelHnorm = 10000;
double magentometerHnorm = 100;
System.out.println("[TrackerServer] Accelerometer Hnorm: " + (accelHnorm = Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size())));
Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 0, accelHnorm, accelBasis, accelAInv);
Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 2, accelHnorm, accelBasis, accelAInv);
System.out.println("[TrackerServer] Magentometer Hnorm: " + (magentometerHnorm = Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size())));
Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 0, magentometerHnorm, magBasis, magAInv);
Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 2, magentometerHnorm, magBasis, magAInv);
System.out.println("float A_B[3] =");
System.out.println(String.format(" {%8.2f,%8.2f,%8.2f},", accelBasis[0], accelBasis[1], accelBasis[2]));
@@ -181,6 +193,9 @@ public class TrackersUDPServer extends Thread {
System.out.println(String.format(" {%9.5f,%9.5f,%9.5f}},", magAInv[6], magAInv[7], magAInv[8]));
System.out.println("float G_off[3] =");
System.out.println(String.format(" {%8.2f, %8.2f, %8.2f}};", gyroOffset[0], gyroOffset[1], gyroOffset[2]));
System.out.println(String.format("Min/Max {%8.2f, %8.2f, %8.2f} / {%8.2f, %8.2f, %8.2f}", magMinX, magMinY, magMinZ, magMaxX, magMaxY, magMaxZ));
System.out.println(String.format("Mag agv {%8.2f, %8.2f, %8.2f},", (magMaxX + magMinX) / 2, (magMaxY + magMinY) / 2, (magMaxZ + magMinZ) / 2));
IMUTracker.CalibrationData data = new IMUTracker.CalibrationData(accelBasis, accelAInv, magBasis, magAInv, gyroOffset);
sensor.tracker.newCalibrationData = data;
@@ -220,6 +235,7 @@ public class TrackersUDPServer extends Thread {
ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN);
try {
socket = new DatagramSocket(port);
socket.setSoTimeout(250);
while(true) {
try {
DatagramPacket recieve = new DatagramPacket(rcvBuffer, rcvBuffer.length);
@@ -230,6 +246,8 @@ public class TrackersUDPServer extends Thread {
synchronized(trackers) {
sensor = trackersMap.get(recieve.getSocketAddress());
}
if(sensor != null)
sensor.lastPacket = System.currentTimeMillis();
int packetId;
switch(packetId = bb.getInt()) {
case 3:
@@ -257,14 +275,20 @@ public class TrackersUDPServer extends Thread {
break;
bb.getLong();
stopCalibration(sensor);
sensor.tracker.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
float x = bb.getFloat();
float z = bb.getFloat();
float y = bb.getFloat();
sensor.tracker.accelVector.set(x, y, z);
break;
case 5:
if(sensor == null)
break;
bb.getLong();
stopCalibration(sensor);
sensor.tracker.magVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
x = bb.getFloat();
z = bb.getFloat();
y = bb.getFloat();
sensor.tracker.magVector.set(x, y, z);
break;
case 6: // PACKET_RAW_CALIBRATION_DATA
if(sensor == null)
@@ -301,17 +325,23 @@ public class TrackersUDPServer extends Thread {
System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
break;
}
if(lastKeepup + 500 < System.currentTimeMillis()) {
lastKeepup = System.currentTimeMillis();
synchronized(trackers) {
for(int i = 0; i < trackers.size(); ++i)
socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, trackers.get(i).address));
}
}
} catch(SocketTimeoutException e) {
} catch(Exception e) {
e.printStackTrace();
}
if(lastKeepup + 500 < System.currentTimeMillis()) {
lastKeepup = System.currentTimeMillis();
synchronized(trackers) {
for(int i = 0; i < trackers.size(); ++i) {
TrackerConnection conn = trackers.get(i);
socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address));
if(conn.lastPacket + 1000 < System.currentTimeMillis())
conn.tracker.setStatus(TrackerStatus.DISCONNECTED);
else
conn.tracker.setStatus(TrackerStatus.OK);
}
}
}
}
} catch(Exception e) {
e.printStackTrace();
@@ -327,6 +357,7 @@ public class TrackersUDPServer extends Thread {
boolean isCalibrating;
private List<double[]> rawCalibrationData = new FastList<>();
private double[] gyroCalibrationData;
public long lastPacket = System.currentTimeMillis();
public TrackerConnection(IMUTracker tracker, SocketAddress address) {
this.tracker = tracker;