// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointCount; jointIndex++)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter end joints highly as they are more noisy
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked ||
jointIndex == (int)KinectInterop.JointType.FootLeft || jointIndex == (int)KinectInterop.JointType.FootRight ||
jointIndex == (int)KinectInterop.JointType.HandLeft || jointIndex == (int)KinectInterop.JointType.HandRight ||
jointIndex == (int)KinectInterop.JointType.HandtipLeft || jointIndex == (int)KinectInterop.JointType.HandtipRight ||
jointIndex == (int)KinectInterop.JointType.ThumbLeft || jointIndex == (int)KinectInterop.JointType.ThumbRight)
//|| jointIndex == (int)KinectInterop.JointType.Head)
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
Vector3 jPosition = bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked ? bodyData.joint[jointIndex].position : Vector3.zero;
bodyData.joint[jointIndex].position = FilterJoint(jPosition, bodyIndex, jointIndex, tempSmoothingParams);
}
bodyData.position = bodyData.joint[0].position;
}
// averages the bodies in the list and returns the single merged body
private KinectInterop.BodyData GetMergedBody(List <KinectInterop.BodyData> alCloseBodies, int bodyIndex, ref List <string> lostUsers)
{
int jointCount = (int)KinectInterop.JointType.Count;
KinectInterop.BodyData mergedBody = new KinectInterop.BodyData(jointCount);
for (int j = 0; j < jointCount; j++)
{
int maxTrackingState = GetBodyJointMaxState(alCloseBodies, j);
CalcAverageBodyJoint(alCloseBodies, j, maxTrackingState, ref mergedBody);
}
mergedBody.liTrackingID = GetMergedBodyId(alCloseBodies, ref lostUsers);
mergedBody.iBodyIndex = bodyIndex;
KinectInterop.JointData pelvisData = mergedBody.joint[0];
mergedBody.bIsTracked = pelvisData.trackingState != KinectInterop.TrackingState.NotTracked;
//Debug.Log(string.Format("MBody {0} Id: {1}, pos: {2}, rot: {3}", bodyIndex, mergedBody.liTrackingID, pelvisData.position, pelvisData.normalRotation.eulerAngles));
mergedBody.kinectPos = pelvisData.kinectPos;
mergedBody.position = pelvisData.position;
mergedBody.orientation = pelvisData.orientation;
mergedBody.normalRotation = pelvisData.normalRotation;
mergedBody.mirroredRotation = pelvisData.mirroredRotation;
KinectInterop.CalcBodyJointDirs(ref mergedBody);
return(mergedBody);
}
/// <summary>
/// Merges the user bodies, detected by the multiple sensors.
/// </summary>
public List <KinectInterop.BodyData> MergeUserBodies(ref ulong lastBodyFrameTime)
{
// get list of all bodies
List <KinectInterop.BodyData> alAllBodies = GetAllBodiesList(ref lastBodyFrameTime);
// build mergeable body sets
List <List <KinectInterop.BodyData> > mergeableBodySets = new List <List <KinectInterop.BodyData> >();
while (alAllBodies.Count > 0)
{
List <KinectInterop.BodyData> alCloseBodies = new List <KinectInterop.BodyData>();
alCloseBodies.Add(alAllBodies[0]);
alAllBodies.RemoveAt(0);
FindOverlappingBodies(ref alCloseBodies, ref alAllBodies, MAX_DISTANCE_TO_CLOSE_BODY, mergeableBodySets.Count);
mergeableBodySets.Add(alCloseBodies);
}
// merge the bodies
List <KinectInterop.BodyData> alMergedBodies = new List <KinectInterop.BodyData>();
List <string> lostUsers = new List <string>();
lostUsers.AddRange(dictSensorUserIdToUserId.Keys);
for (int i = 0; i < mergeableBodySets.Count; i++)
{
KinectInterop.BodyData mergedBody = GetMergedBody(mergeableBodySets[i], i, ref lostUsers);
alMergedBodies.Add(mergedBody);
}
// clean up
mergeableBodySets.Clear();
mergeableBodySets = null;
alAllBodies = null;
if (lostUsers.Count > 0)
{
foreach (string sensorUserId in lostUsers)
{
if (dictSensorUserIdToUserId.ContainsKey(sensorUserId) && (Time.time - dictSensorUserIdToLastUsed[sensorUserId]) >= WAIT_TIME_BEFORE_REMOVAL)
{
dictSensorUserIdToUserId.Remove(sensorUserId);
dictSensorUserIdToFirstUsed.Remove(sensorUserId);
dictSensorUserIdToLastUsed.Remove(sensorUserId);
//Debug.Log("Removed lost sensor-user-id from dict: " + sensorUserId);
}
}
lostUsers.Clear();
lostUsers = null;
}
return(alMergedBodies);
}
// returns list of all bodies, tracked by all sensors
private List <KinectInterop.BodyData> GetAllBodiesList(ref ulong lastBodyFrameTime, BoneOrientationConstraints boneConstraints)
{
List <KinectInterop.BodyData> alAllBodies = new List <KinectInterop.BodyData>();
for (int s = 0; s < sensorDatas.Count; s++)
{
if (SINGLE_SENSOR_INDEX >= 0 && s != SINGLE_SENSOR_INDEX)
{
continue;
}
KinectInterop.SensorData sensorData = sensorDatas[s];
uint sensorBodyCount = sensorData.trackedBodiesCount;
if (sensorBodyCount > 0 && sensorData.lastBodyFrameTime > lastBodyFrameTime)
{
lastBodyFrameTime = sensorData.lastBodyFrameTime;
}
for (uint b = 0; b < sensorBodyCount; b++)
{
KinectInterop.BodyData bodyData = new KinectInterop.BodyData((int)KinectInterop.JointType.Count);
sensorData.alTrackedBodies[b].CopyTo(ref bodyData);
bodyData.sensorIndex = s;
// filter orientation constraints
if (boneConstraints != null)
{
boneConstraints.Constrain(ref bodyData);
}
alAllBodies.Add(bodyData);
}
//break;
}
if (alAllBodies.Count > 0)
{
//Debug.Log("Found " + alAllBodies.Count + " total bodies.");
}
return(alAllBodies);
}
//// Update the filter with a new frame of data and smooth.
//public void UpdateFilter(ref KinectInterop.BodyData[] alTrackedBodies)
//{
// if (!init)
// {
// // initialize with by-default parameters
// Init();
// }
// if (smoothingType == SmoothingType.None)
// return;
// SmoothParameters tempSmoothingParams = new SmoothParameters();
// tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
// tempSmoothingParams.correction = this.smoothParameters.correction;
// tempSmoothingParams.prediction = this.smoothParameters.prediction;
// int bodyCount = alTrackedBodies != null ? alTrackedBodies.Length : 0;
// for (int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
// {
// if (alTrackedBodies[bodyIndex].bIsTracked)
// {
// FilterBodyJoints(ref alTrackedBodies[bodyIndex], /**bodyIndex*/ alTrackedBodies[bodyIndex].iBodyIndex, tempSmoothingParams);
// }
// }
//}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectInterop.BodyData bodyData)
{
if (!init)
{
// initialize with by-default parameters
Init();
}
if (smoothingType == SmoothingType.None)
{
return;
}
SmoothParameters tempSmoothingParams = new SmoothParameters();
tempSmoothingParams.smoothing = smoothParameters.smoothing;
tempSmoothingParams.correction = smoothParameters.correction;
tempSmoothingParams.prediction = smoothParameters.prediction;
if (bodyData.bIsTracked)
{
// get body index
int bodyIndex = GetUserIndex(bodyData.liTrackingID);
if (bodyIndex < 0)
{
bodyIndex = GetFreeIndex();
if (bodyIndex >= 0)
{
history[bodyIndex].userId = bodyData.liTrackingID;
}
//Debug.Log("Created history for userId: " + history[bodyIndex].userId + ", index: " + bodyIndex + ", time: " + DateTime.UtcNow);
}
// filter
if (bodyIndex >= 0)
{
FilterBodyJoints(ref bodyData, bodyIndex, tempSmoothingParams);
}
}
// free unused history
//CleanUpUserHistory();
}
// Apply the orientation constraints
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager kinectManager = KinectManager.Instance;
frameNum++;
for (int i = 0; i < jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.Pelvis || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
continue;
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
continue;
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
continue;
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
//Quaternion rotDefaultN = Quaternion.identity; // Quaternion.FromToRotation(KinectInterop.JointBaseDir[prevJoint], KinectInterop.JointBaseDir[jc.thisJoint]);
//rotParentN = rotParentN * rotDefaultN;
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
bool isConstrained = false;
//string sDebug = string.Empty;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion rotLimited = rotLocalN;
switch (ac.consType)
{
case 0:
break;
case CT.LimA:
eulerAnglesN = LimitAngles(eulerAnglesN, ac.axis, ac.angleMin, ac.angleMax);
rotLimited = Quaternion.Euler(eulerAnglesN);
break;
case CT.LimST:
rotLimited = LimitSwing(rotLocalN, ac.axis, ac.angleMin);
rotLimited = LimitTwist(rotLimited, ac.axis, ac.angleMax);
break;
case CT.LimH:
float lastAngle = bodyData.joint[jc.thisJoint].lastAngle;
rotLimited = LimitHinge(rotLocalN, ac.axis, ac.angleMin, ac.angleMax, ref lastAngle);
bodyData.joint[jc.thisJoint].lastAngle = lastAngle;
break;
default:
throw new Exception("Undefined constraint type found: " + (int)ac.consType);
}
if (rotLimited != rotLocalN)
{
rotLocalN = rotLimited;
isConstrained = true;
}
}
//if (sDebug.Length > 0)
//{
// if (debugText != null && jc.thisJoint == (int)KinectInterop.JointType.ElbowLeft)
// {
// // debugText.text = sDebug;
// }
// Debug.Log(sDebug);
//}
if (isConstrained)
{
rotJointN = rotParentN * rotLocalN;
Vector3 eulerJoint = rotJointN.eulerAngles;
Vector3 eulerJointM = new Vector3(eulerJoint.x, -eulerJoint.y, -eulerJoint.z);
Quaternion rotJointM = Quaternion.Euler(eulerJointM);
// put it back into the bone orientations
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
}
}
}
// returns averaged position of a body joint in a list of bodies
private void CalcAverageBodyJoint(List <KinectInterop.BodyData> alBodyList, int jointIndex, int maxTrackingState,
ref KinectInterop.BodyData bodyData)
{
Vector3 avgJointPos = Vector3.zero;
Vector3 firstKinectPos = Vector3.zero;
Quaternion avgJointRot = Quaternion.identity;
Quaternion firstJointOri = Quaternion.identity;
Quaternion firstJointRot = Quaternion.identity;
float x = 0f, y = 0f, z = 0f, w = 0f;
float jointAvgCount = 0f;
int bodyCount = alBodyList.Count;
//bool bJointOnePassOnly = Array.Exists(_OnePassJoints, j => j == jointIndex);
for (int i = 0; i < bodyCount; i++)
{
////if (bJointOnePassOnly)
//if (jointIndex == (int)KinectInterop.JointType.WristLeft)
//{
// Debug.Log(string.Format("{0:F3} {1}: {2}_{3}, state: {4}, pos: {5}, rot: {6}", Time.time, (KinectInterop.JointType)jointIndex,
// alBodyList[i].sensorIndex, alBodyList[i].liTrackingID, alBodyList[i].joint[jointIndex].trackingState,
// alBodyList[i].joint[jointIndex].position, alBodyList[i].joint[jointIndex].mirroredRotation.eulerAngles));
//}
//if (SINGLE_SENSOR_INDEX >= 0 && alBodyList[i].sensorIndex != SINGLE_SENSOR_INDEX)
// continue;
KinectInterop.TrackingState jointState = alBodyList[i].joint[jointIndex].trackingState;
//if ((int)jointState == maxTrackingState)
if (jointState != KinectInterop.TrackingState.NotTracked)
{
Quaternion jointRot = alBodyList[i].joint[jointIndex].normalRotation;
if (avgJointPos == Vector3.zero)
{
firstKinectPos = alBodyList[i].joint[jointIndex].kinectPos;
firstJointOri = alBodyList[i].joint[jointIndex].orientation;
firstJointRot = jointRot;
}
if (jointIndex == 0)
{
//Debug.Log(string.Format("Body Id: {0}_{1}, pos: {2}, rot: {3}", alBodyList[i].sensorIndex, alBodyList[i].liTrackingID, alBodyList[i].joint[jointIndex].position, alBodyList[i].joint[jointIndex].normalRotation.eulerAngles));
}
float jointWeight = 1f; // jointState != KinectInterop.TrackingState.Inferred ? 1f : 0.5f;
avgJointPos += alBodyList[i].joint[jointIndex].position * jointWeight;
if (Quaternion.Dot(jointRot, firstJointRot) < 0f)
{
jointRot = new Quaternion(-jointRot.x, -jointRot.y, -jointRot.z, -jointRot.w); // inverse the sign
}
if (jointWeight < 0.9f)
{
jointRot = Quaternion.Slerp(Quaternion.identity, jointRot, jointWeight);
}
x += jointRot.x; y += jointRot.y; z += jointRot.z; w += jointRot.w;
jointAvgCount += jointWeight; // (jointState != KinectInterop.TrackingState.Inferred ? 1f : 0.5f);
//if(bJointOnePassOnly)
// break;
}
}
if (jointAvgCount > 0)
{
float addDet = 1f / jointAvgCount;
avgJointPos = avgJointPos * addDet;
x *= addDet; y *= addDet; z *= addDet; w *= addDet;
float lengthD = 1.0f / (w * w + x * x + y * y + z * z);
x *= lengthD; y *= lengthD; z *= lengthD; w *= lengthD;
avgJointRot = new Quaternion(x, y, z, w);
}
KinectInterop.JointData jointData = bodyData.joint[jointIndex];
jointData.trackingState = (KinectInterop.TrackingState)maxTrackingState;
jointData.kinectPos = firstKinectPos;
jointData.position = avgJointPos;
jointData.orientation = firstJointOri;
jointData.normalRotation = avgJointRot;
Vector3 mirroredRot = avgJointRot.eulerAngles;
mirroredRot.y = -mirroredRot.y;
mirroredRot.z = -mirroredRot.z;
jointData.mirroredRotation = Quaternion.Euler(mirroredRot);
bodyData.joint[jointIndex] = jointData;
}
