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 161 Packages Projects Releases Wiki Activity

Fix sensor attachment alignment correction to use quaternions and separate it from sensor yaw difference (should work better with feet tracking)

Browse Source
This commit is contained in:
Eiren Rain
2021-05-27 21:31:02 +03:00
parent 5ad133656b
commit 9dc3fb20dc
4 changed files with 42 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java
View File

@@ -74,7 +74,7 @@ public class HumanPoseProcessor {
@VRServerThread
private void addTracker(Tracker tracker, TrackerBodyPosition position) {
AdjustedTracker tt = new AdjustedFullTracker(tracker);
AdjustedTracker tt = new AdjustedTracker(tracker);
trackers.put(position, tt);
server.registerTracker(tt);
@@ -135,7 +135,7 @@ public class HumanPoseProcessor {
Iterator<AdjustedTracker> iterator = trackers.values().iterator();
while(iterator.hasNext()) {
AdjustedTracker tt = iterator.next();
tt.adjust(hmdRotation);
tt.adjustFull(hmdRotation);
TrackerConfig config = server.getTrackerConfig(tt);
tt.saveConfig(config);

42
src/main/java/io/eiren/vr/trackers/AdjustedFullTracker.java
View File

@@ -1,42 +0,0 @@
package io.eiren.vr.trackers;
import com.jme3.math.Quaternion;
public class AdjustedFullTracker extends AdjustedYawTracker {
private final float[] angles = new float[3];
private float yawCorrection = 0;
private float pitchCorrection = 0;
private float rollCorrection = 0;
public AdjustedFullTracker(Tracker tracker) {
super(tracker);
}
@Override
public void adjust(Quaternion reference) {
float[] angles = this.angles;
reference.toAngles(angles);
// Use only yaw HMD rotation
angles[0] = 0;
angles[2] = 0;
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
float[] angles2 = new float[3];
sensorRotation.toAngles(angles2);
yawCorrection = angles[1] - angles2[1];
pitchCorrection = angles[0] - angles2[0];
rollCorrection = angles[2] - angles2[2];
}
@Override
protected void adjustInternal(Quaternion store) {
float[] angles = this.angles;
store.toAngles(angles);
angles[0] += pitchCorrection;
angles[1] += yawCorrection;
angles[2] += rollCorrection;
store.fromAngles(angles);
}
}

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

@@ -4,13 +4,17 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
public abstract class AdjustedTracker implements Tracker {
public class AdjustedTracker implements Tracker {
public final Tracker tracker;
private final Quaternion smoothedQuaternion = new Quaternion();
private float[] angles = new float[3];
public final Quaternion adjustmentYaw = new Quaternion();
public final Quaternion adjustmentAttachment = new Quaternion();
protected float[] lastAngles = new float[3];
public float smooth = 0 * FastMath.DEG_TO_RAD;
private final float[] angles = new float[3];
private float pitchCorrection = 0;
private float rollCorrection = 0;
protected float confidenceMultiplier = 1.0f;
@@ -25,8 +29,41 @@ public abstract class AdjustedTracker implements Tracker {
@Override
public void saveConfig(TrackerConfig config) {
}
public void adjustFull(Quaternion reference) {
adjustYaw(reference);
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
adjustmentYaw.mult(sensorRotation, sensorRotation);
adjustmentAttachment.set(sensorRotation).inverseLocal();
}
public abstract void adjust(Quaternion reference);
public void adjustYaw(Quaternion reference) {
Quaternion targetTrackerRotation = new Quaternion(reference);
// Use only yaw HMD rotation
float[] angles = new float[3];
targetTrackerRotation.toAngles(angles);
targetTrackerRotation.fromAngles(0, angles[1], 0);
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
sensorRotation.toAngles(angles);
sensorRotation.fromAngles(0, angles[1], 0);
adjustmentYaw.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
lastAngles[0] = 1000;
}
protected void adjustInternal(Quaternion store) {
store.multLocal(adjustmentAttachment);
adjustmentYaw.mult(store, store);
}
@Override
public boolean getRotation(Quaternion store) {
@@ -43,8 +80,6 @@ public abstract class AdjustedTracker implements Tracker {
adjustInternal(store);
return true;
}
protected abstract void adjustInternal(Quaternion store);
@Override
public boolean getPosition(Vector3f store) {

38
src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java
View File

@@ -1,38 +0,0 @@
package io.eiren.vr.trackers;
import com.jme3.math.Quaternion;
public class AdjustedYawTracker extends AdjustedTracker {
public final Quaternion adjustment = new Quaternion();
public AdjustedYawTracker(Tracker tracker) {
super(tracker);
}
@Override
public void adjust(Quaternion reference) {
Quaternion targetTrackerRotation = new Quaternion(reference);
// Use only yaw HMD rotation
float[] angles = new float[3];
targetTrackerRotation.toAngles(angles);
targetTrackerRotation.fromAngles(0, angles[1], 0);
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
sensorRotation.toAngles(angles);
sensorRotation.fromAngles(0, angles[1], 0);
adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
lastAngles[0] = 1000;
}
@Override
protected void adjustInternal(Quaternion store) {
adjustment.mult(store, store);
}
}