diff --git a/Unity/OpenSeeIKTarget.cs b/Unity/OpenSeeIKTarget.cs index dafc7c2..e8c1dcf 100644 --- a/Unity/OpenSeeIKTarget.cs +++ b/Unity/OpenSeeIKTarget.cs @@ -10,6 +10,8 @@ public class OpenSeeIKTarget : MonoBehaviour public OpenSee openSee; [Tooltip("When this is set to another game object, its kinematic interpolation component will get updated from values of this.")] public OpenSeeKinematicInterpolation kinematicInterpolation; + [Tooltip("When set, the this object will follow the specified transform instead and automatically recalibrate based on it.")] + public Transform externalTarget = null; [Tooltip("This can be used to skip the first few tracking frames.")] public int skipFirst = 5; [Tooltip("When set to true, the next rotation and translation values will be set as zero rotation and zero translation.")] @@ -23,8 +25,6 @@ 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.")] @@ -85,17 +85,7 @@ 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; + private Transform lastExternalTarget = null; public void Calibrate() { calibrate = true; @@ -119,30 +109,11 @@ public class OpenSeeIKTarget : MonoBehaviour } 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; - } + return Vector3.Lerp(a, b, 1f - smooth); } 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); - } + return Quaternion.Lerp(a, b, 1f - smooth); } void Interpolate() { @@ -166,43 +137,33 @@ public class OpenSeeIKTarget : MonoBehaviour void RunUpdate() { var openSeeData = openSee.GetOpenSeeData(faceId); - if (openSeeData == null || openSeeData.fit3DError > openSee.maxFit3DError) - return; - if (openSeeData.time > updated) { - frameTime = (float)(openSeeData.time - updated); - updated = openSeeData.time; - trackingFrames++; - if (trackingFrames < skipFirst) - return; - } else { - Interpolate(); - return; - } + Quaternion convertedQuaternion; + Vector3 t; + Vector3 convertedTranslation; - 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 (externalTarget == null) { + if (openSeeData == null || openSeeData.fit3DError > openSee.maxFit3DError) + return; + if (openSeeData.time > updated) { + updated = openSeeData.time; + trackingFrames++; + if (trackingFrames < skipFirst) + return; + } else { + Interpolate(); + return; + } + + convertedQuaternion = new Quaternion(-openSeeData.rawQuaternion.y, -openSeeData.rawQuaternion.x, openSeeData.rawQuaternion.z, openSeeData.rawQuaternion.w); + t = openSeeData.translation; + t.x = -t.x; + t.z = -t.z; + convertedTranslation = new Vector3(t.x, t.y, t.z); + } else { + convertedQuaternion = externalTarget.localRotation; + convertedTranslation = externalTarget.localPosition; + t = convertedTranslation; } - 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; - t.x = -t.x; - t.z = -t.z; - Vector3 convertedTranslation = new Vector3(t.x, t.y, t.z); // Check for angular outliers if (gotAccepted) { @@ -267,6 +228,9 @@ public class OpenSeeIKTarget : MonoBehaviour outlierSkipsDistance += 1; return; } + + calibrate = calibrate || (externalTarget != lastExternalTarget); + lastExternalTarget = externalTarget; if (calibrate) { dR = convertedQuaternion; @@ -302,7 +266,7 @@ public class OpenSeeIKTarget : MonoBehaviour lastT = transform.localPosition; lastR = transform.localRotation; if (interpolateState > 0) { - currentT = FilterPos(currentT, (t - dT) * translationScale); + currentT = FilterPos(transform.localPosition, (t - dT) * translationScale); currentR = FilterRot(transform.localRotation, convertedQuaternion * dR); } else { currentT = (t - dT) * translationScale; @@ -348,34 +312,6 @@ 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; - } - } } } \ No newline at end of file diff --git a/Unity/OpenSeeIKTarget.cs.meta b/Unity/OpenSeeIKTarget.cs.meta index 94933f1..dd50d92 100644 --- a/Unity/OpenSeeIKTarget.cs.meta +++ b/Unity/OpenSeeIKTarget.cs.meta @@ -4,7 +4,7 @@ MonoImporter: externalObjects: {} serializedVersion: 2 defaultReferences: [] - executionOrder: -1 + executionOrder: -5 icon: {instanceID: 0} userData: assetBundleName: diff --git a/Unity/OpenSeeKinematicInterpolation.cs b/Unity/OpenSeeKinematicInterpolation.cs index 99b7034..7b347a8 100644 --- a/Unity/OpenSeeKinematicInterpolation.cs +++ b/Unity/OpenSeeKinematicInterpolation.cs @@ -7,6 +7,8 @@ namespace OpenSee { public class OpenSeeKinematicInterpolation : MonoBehaviour { + public float interpolateClamp = 1f; + private Vector3 lastPos; private Vector3 lastPosV; private double lastT; @@ -29,15 +31,19 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour private double updateT; private double interpolateT; + private int ready = 0; public void UpdateKI(double newT, Vector3 newPos, Quaternion newRot) { Interpolate(); + if (newT - currentT > 3f) + ready = 0; + currentPos = transform.localPosition; currentRot = OpenSeeIKTarget.PrettyEuler(currentRot, transform.localRotation.eulerAngles); float dTNow = (float)(interpolateT - updateT); - if (interpolateT - updateT > 0) { - currentPosV = (currentPos - lastPos) / dTNow; - currentRotV = (currentRot - lastRot) / dTNow; + if (interpolateT - updateT > 0 || ready < 1) { + currentPosV = (currentPos - lastPos) / dT; + currentRotV = (currentRot - lastRot) / dT; } else { currentPosV = Vector3.zero; currentRotV = Vector3.zero; @@ -64,6 +70,8 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour coeffRotB = ((6f*(currentRotV * dT + dT * lastRotV - 2f * currentRot + 2f * lastRot)) / dTPow3)/6f; updateT = Time.time; + if (ready <= 2) + ready++; } float InterpolateValue(float dTNow, float dTNowPow2, float dTNowPow3, float a, float b, float x1, float x2, float v1, float v2) { @@ -72,12 +80,12 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour void Interpolate() { interpolateT = Time.time; - if (interpolateT == updateT) + if (interpolateT == updateT || ready <= 2) return; float dTNow = (float)(interpolateT - updateT); if (dTNow >= dT) { - transform.localPosition = Vector3.LerpUnclamped(lastPos, currentPos, Mathf.Min((float)(dTNow / dT), 2f)); - transform.localEulerAngles = Vector3.LerpUnclamped(lastRot, currentRot, Mathf.Min((float)(dTNow / dT), 2f)); + transform.localPosition = Vector3.LerpUnclamped(lastPos, currentPos, Mathf.Min((float)(dTNow / dT), interpolateClamp)); + transform.localEulerAngles = Vector3.LerpUnclamped(lastRot, currentRot, Mathf.Min((float)(dTNow / dT), interpolateClamp)); } else { float dTNowPow2 = Mathf.Pow(dTNow, 2); float dTNowPow3 = Mathf.Pow(dTNow, 3); @@ -92,7 +100,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour } } - public void LateUpdate() { + public void Update() { Interpolate(); } } diff --git a/Unity/OpenSeeKinematicInterpolation.cs.meta b/Unity/OpenSeeKinematicInterpolation.cs.meta index 571dd6e..1a31547 100644 --- a/Unity/OpenSeeKinematicInterpolation.cs.meta +++ b/Unity/OpenSeeKinematicInterpolation.cs.meta @@ -4,7 +4,7 @@ MonoImporter: externalObjects: {} serializedVersion: 2 defaultReferences: [] - executionOrder: 0 + executionOrder: -1 icon: {instanceID: 0} userData: assetBundleName: