copilot/OpenSeeFace
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Disabled ThunderSVM loading and added better interpolation.

Browse source
This commit is contained in:
Emiliana 2020-09-26 20:21:46 +02:00
parent 5abe5ba633
commit eaaf036e76
4 changed files with 270 additions and 150 deletions
Download patch file Download diff file Expand all files Collapse all files

6
Unity/OpenSeeExpression.cs
View file

@ -166,10 +166,10 @@ public class OpenSeeExpression : MonoBehaviour
oser = formatter.Deserialize(gzipStream) as OpenSeeExpressionRepresentation;
}
expressions = oser.expressions;
if (oser.thunderSVM)
/*if (oser.thunderSVM)
model = new ThunderSVMModel(oser.modelBytes);
else
model = new SVMModel(oser.modelBytes);
else*/
model = new SVMModel(oser.modelBytes);
classLabels = oser.classLabels;
indices = oser.indices;
//pointSelection = oser.pointSelection;

121
Unity/OpenSeeIKTarget.cs
View file

@ -22,6 +22,8 @@ public class OpenSeeIKTarget : MonoBehaviour
[Tooltip("This smoothed out the detected motion. Values can be between 0 (no smoothing) and 1 (no motion). The appropriate value probably depends on the camera's frame rate.")]
[Range(0, 1)]
public float smooth = 0.2f;
[Tooltip("When set to true, a Kalman filter is used for smoothing.")]
public bool kalman = false;
[Tooltip("When enabled, tracking will lag by one video capture frame, but the captured motion information is interpolated between frames, making things look smoother and less jittery and jumpy.")]
public bool interpolate = true;
[Tooltip("When set to true, the transform will be updated in FixedUpdate, otherwise it will be updated in Update.")]
@ -82,9 +84,65 @@ public class OpenSeeIKTarget : MonoBehaviour
private bool skippedPosition = false;
private float skipStartTimePosition = -1f;
// Kalman
private float frameTime = 0f;
private bool kalmanInit = false;
private float kalmanSmooth = -1f;
private KF kalmanPVX;
private KF kalmanPVY;
private KF kalmanPVZ;
private KF kalmanRVX;
private KF kalmanRVY;
private KF kalmanRVZ;
private Vector3 kalmanLastRot = Vector3.zero;
public void Calibrate() {
calibrate = true;
}
public static Vector3 PrettyEuler(Vector3 last, Vector3 current) {
// Ew, but how would you do componentwise filtering/interpolation with quaternions?
if (Mathf.Abs(current.x - last.x) > Mathf.Abs((current.x - 360f) - last.x))
current.x -= 360f;
else if (Mathf.Abs(current.x - last.x) > Mathf.Abs((current.x + 360f) - last.x))
current.x += 360f;
if (Mathf.Abs(current.y - last.y) > Mathf.Abs((current.y - 360f) - last.y))
current.y -= 360f;
else if (Mathf.Abs(current.y - last.y) > Mathf.Abs((current.y + 360f) - last.y))
current.x += 360f;
if (Mathf.Abs(current.z - last.z) > Mathf.Abs((current.z - 360f) - last.z))
current.z -= 360f;
else if (Mathf.Abs(current.z - last.z) > Mathf.Abs((current.z + 360f) - last.z))
current.z += 360f;
return current;
}
Vector3 FilterPos(Vector3 a, Vector3 b) {
if (!kalman || frameTime > 1f)
return Vector3.Lerp(a, b, 1f - smooth);
else {
Vector3 v = (b - a) / frameTime;
v.x = kalmanPVX.Update(v.x);
v.y = kalmanPVY.Update(v.y);
v.z = kalmanPVZ.Update(v.z);
return a + v * frameTime;
}
}
Quaternion FilterRot(Quaternion a, Quaternion b) {
if (!kalman || frameTime > 1f)
return Quaternion.Lerp(a, b, 1f - smooth);
else {
Vector3 rotA = PrettyEuler(kalmanLastRot, a.eulerAngles);
Vector3 rotB = PrettyEuler(rotA, b.eulerAngles);
kalmanLastRot = rotA;
Vector3 v = Mathf.Deg2Rad * (rotB - rotA) / frameTime;
v.x = kalmanRVX.Update(v.x);
v.y = kalmanRVY.Update(v.y);
v.z = kalmanRVZ.Update(v.z);
return Quaternion.Euler(rotA + Mathf.Rad2Deg * v * frameTime);
}
}
void Interpolate() {
if (!interpolate || interpolateState < 2)
@ -110,6 +168,7 @@ public class OpenSeeIKTarget : MonoBehaviour
if (openSeeData == null || openSeeData.fit3DError > openSee.maxFit3DError)
return;
if (openSeeData.time > updated) {
frameTime = (float)(openSeeData.time - updated);
updated = openSeeData.time;
trackingFrames++;
if (trackingFrames < skipFirst)
@ -118,6 +177,25 @@ public class OpenSeeIKTarget : MonoBehaviour
Interpolate();
return;
}
if (!kalmanInit) {
kalmanPVX = new KF(smooth);
kalmanPVY = new KF(smooth);
kalmanPVZ = new KF(smooth);
kalmanRVX = new KF(smooth);
kalmanRVY = new KF(smooth);
kalmanRVZ = new KF(smooth);
kalmanInit = true;
}
if (smooth != kalmanSmooth) {
kalmanPVX.SetSmoothness(smooth);
kalmanPVY.SetSmoothness(smooth);
kalmanPVZ.SetSmoothness(smooth);
kalmanRVX.SetSmoothness(smooth);
kalmanRVY.SetSmoothness(smooth);
kalmanRVZ.SetSmoothness(smooth);
kalmanSmooth = smooth;
}
Quaternion convertedQuaternion = new Quaternion(-openSeeData.rawQuaternion.y, -openSeeData.rawQuaternion.x, openSeeData.rawQuaternion.z, openSeeData.rawQuaternion.w);
Vector3 t = openSeeData.translation;
@ -219,12 +297,12 @@ public class OpenSeeIKTarget : MonoBehaviour
avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f);
interpolationCount = 0;
averageInterpolations = avgInterps;
lastT = transform.localPosition;
lastR = transform.localRotation;
if (interpolateState > 0) {
currentT = Vector3.Lerp(currentT, (t - dT) * translationScale, 1f - smooth);
currentR = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
currentT = FilterPos(currentT, (t - dT) * translationScale);
currentR = FilterRot(transform.localRotation, convertedQuaternion * dR);
} else {
currentT = (t - dT) * translationScale;
currentR = convertedQuaternion * dR;
@ -235,11 +313,14 @@ public class OpenSeeIKTarget : MonoBehaviour
if (interpolate) {
Interpolate();
} else {
transform.localPosition = Vector3.Lerp(transform.localPosition, (t - dT) * translationScale, 1f - smooth);
transform.localRotation = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
transform.localPosition = FilterPos(transform.localPosition, (t - dT) * translationScale);
transform.localRotation = FilterRot(transform.localRotation, convertedQuaternion * dR);
}
if (kinematicInterpolation != null && kinematicInterpolation.gameObject != gameObject) {
kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation);
if (interpolate)
kinematicInterpolation.UpdateKI(updated, currentT, currentR);
else
kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation);
}
if (driftBack) {
@ -266,6 +347,34 @@ public class OpenSeeIKTarget : MonoBehaviour
if (!fixedUpdate)
RunUpdate();
}
class KF {
private float R, P, Q, x;
public KF(float smooth) {
P = 1f;
x = 0f;
SetSmoothness(smooth);
}
public void SetSmoothness(float smooth) {
P = 1f;
Q = 0.3f;// * (Mathf.Exp(3f * smooth) + 1f);
R = 0.000001f + Mathf.Exp(smooth * 5f);
}
public float Update(float input) {
//x = x * A;
//P = A * P * A;
float K = P / (P + R);
x = x + K * (input - x);
//Debug.Log("P = " + P + " 1-K = " + (1f - K));
P = (1f - K) * P + Q;
return x;
}
}
}
}

28
Unity/OpenSeeKinematicInterpolation.cs
View file

@ -3,6 +3,8 @@ using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace OpenSee {
public class OpenSeeKinematicInterpolation : MonoBehaviour
{
private Vector3 lastPos;
@ -13,7 +15,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
private Vector3 currentPosV;
private double currentT = 0f;
private Vector3 lastRot;
private Vector3 lastRot = Vector3.zero;
private Vector3 lastRotV;
private Vector3 currentRot;
@ -28,27 +30,12 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
private double updateT;
private double interpolateT;
public void PrettyEuler() {
// Ew, but how would you do this interpolation with quaternions?
if (currentRot.x > 270f && lastRot.x < 90f)
currentRot.x -= 360f;
if (currentRot.y > 270f && lastRot.y < 90f)
currentRot.y -= 360f;
if (currentRot.z > 270f && lastRot.z < 90f)
currentRot.z -= 360f;
if (currentRot.x < 90f && lastRot.x > 270f)
lastRot.x -= 360f;
if (currentRot.y < 90f && lastRot.y > 270f)
lastRot.y -= 360f;
if (currentRot.z < 90f && lastRot.z > 270f)
lastRot.z -= 360f;
}
public void UpdateKI(double newT, Vector3 newPos, Quaternion newRot) {
Interpolate();
currentPos = transform.localPosition;
currentRot = OpenSeeIKTarget.PrettyEuler(currentRot, transform.localRotation.eulerAngles);
float dTNow = (float)(interpolateT - updateT);
if (interpolateT - updateT > 0) {
float dTNow = (float)(interpolateT - updateT);
currentPosV = (currentPos - lastPos) / dTNow;
currentRotV = (currentRot - lastRot) / dTNow;
} else {
@ -63,8 +50,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
lastT = currentT;
currentPos = newPos;
currentRot = newRot.eulerAngles;
PrettyEuler();
currentRot = OpenSeeIKTarget.PrettyEuler(lastRot, newRot.eulerAngles);
currentT = newT;
dT = (float)(currentT - lastT);
currentPosV = (currentPos - lastPos) / dT;
@ -109,4 +95,6 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
public void LateUpdate() {
Interpolate();
}
}
}