dwelling act 4 (live motion cap w/ kinect azure)
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

615 lines
26 KiB

1 year ago
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace com.rfilkov.kinect
{
/// <summary>
/// KinectUserBodyMerger merges user bodies detected by multiple connected sensors.
/// Attribution: This class is based on the BodyMerger-class, provided by Cy-Fighter.com (http://cy-fighter.com/).
/// See: KinectUserBodyMerger-Attribution.txt
/// </summary>
public class KinectUserBodyMerger
{
// references to sensor-data objects
private List<KinectInterop.SensorData> sensorDatas = null;
private Dictionary<ulong, ulong>[] adUserIdToSensorTrackingId = null;
// sensor user Id to merged user Id
private Dictionary<string, ulong> dictSensorUserIdToUserId = new Dictionary<string, ulong>();
private Dictionary<string, float> dictSensorUserIdToLastUsed = new Dictionary<string, float>();
private Dictionary<string, float> dictSensorUserIdToFirstUsed = new Dictionary<string, float>();
private ulong nextUserId = 0;
// first sensor data
private KinectInterop.SensorData firstSensorData = null;
// log-file name
//private string logFileName = string.Empty;
// maximum distance between close bodies
private const float MAX_DISTANCE_TO_CLOSE_BODY = 0.35f;
// if only one sensor needs to be considered, set its index here
private const int SINGLE_SENSOR_INDEX = -1;
// wait time in seconds, before sensor user gets removed from the dictionaries
private const float WAIT_TIME_BEFORE_REMOVAL = 0.3f;
public KinectUserBodyMerger(List<KinectInterop.SensorData> sensorDatas)
{
this.sensorDatas = sensorDatas;
if(sensorDatas.Count > 0)
{
firstSensorData = sensorDatas[0];
}
adUserIdToSensorTrackingId = new Dictionary<ulong, ulong>[sensorDatas.Count];
for(int i = 0; i < adUserIdToSensorTrackingId.Length; i++)
{
adUserIdToSensorTrackingId[i] = new Dictionary<ulong, ulong>();
}
dictSensorUserIdToUserId.Clear();
dictSensorUserIdToFirstUsed.Clear();
dictSensorUserIdToLastUsed.Clear();
nextUserId = 1;
//logFileName = DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".log";
//Debug.Log("Logging merger to: " + logFileName);
}
/// <summary>
/// Merges the user bodies, detected by the multiple sensors.
/// </summary>
public List<KinectInterop.BodyData> MergeUserBodies(ref ulong lastBodyFrameTime, BoneOrientationConstraints boneConstraints)
{
// get list of all bodies
List<KinectInterop.BodyData> alAllBodies = GetAllBodiesList(ref lastBodyFrameTime, boneConstraints);
//System.Text.StringBuilder sbDebug = new System.Text.StringBuilder();
//sbDebug.AppendFormat("Time: {0:F3} - {1:T}", Time.time, DateTime.Now);
//sbDebug.AppendFormat("\nAll({0}): ", alAllBodies.Count);
//foreach (KinectInterop.BodyData body in alAllBodies)
// sbDebug.AppendFormat("{0}_{1} {2} ", body.sensorIndex, body.liTrackingID, body.position);
// build mergeable body sets
List<List<KinectInterop.BodyData>> mergeableBodySets = new List<List<KinectInterop.BodyData>>();
//sbDebug.Append("\nMergeable: ");
while (alAllBodies.Count > 0)
{
List<KinectInterop.BodyData> alCloseBodies = new List<KinectInterop.BodyData>();
alCloseBodies.Add(alAllBodies[0]);
alAllBodies.RemoveAt(0);
FindOverlappingBodies(alCloseBodies, alAllBodies, MAX_DISTANCE_TO_CLOSE_BODY, mergeableBodySets.Count);
mergeableBodySets.Add(alCloseBodies);
//sbDebug.AppendFormat("({0}): ", alCloseBodies.Count);
//foreach (KinectInterop.BodyData body in alCloseBodies)
// sbDebug.AppendFormat("{0}_{1} {2} D:{3:F2} ", body.sensorIndex, body.liTrackingID, body.position, Vector3.Distance(alCloseBodies[0].position, body.position));
//sbDebug.Append("; ");
}
// merge the bodies
List<KinectInterop.BodyData> alMergedBodies = new List<KinectInterop.BodyData>();
for(int i = 0; i < adUserIdToSensorTrackingId.Length; i++)
{
adUserIdToSensorTrackingId[i].Clear();
}
Matrix4x4 world2SensorMat = Matrix4x4.identity;
if (firstSensorData != null)
{
Matrix4x4 sensor2WorldMat = firstSensorData.sensorInterface.GetSensorToWorldMatrix();
world2SensorMat = sensor2WorldMat.inverse;
}
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 world2SensorMat, lostUsers, alMergedBodies);
alMergedBodies.Add(mergedBody);
}
//sbDebug.AppendFormat("\nMerged({0}): ", alMergedBodies.Count);
//foreach (KinectInterop.BodyData body in alMergedBodies)
// sbDebug.AppendFormat("{0} {1} ", body.liTrackingID, body.position);
// clean up
mergeableBodySets.Clear();
mergeableBodySets = null;
alAllBodies = null;
if (lostUsers.Count > 0)
{
//sbDebug.AppendFormat("\nLost({0}): ", lostUsers.Count);
foreach (string sensorUserId in lostUsers)
{
//sbDebug.Append(sensorUserId);
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);
//sbDebug.Append("*");
}
//sbDebug.Append(" ");
}
lostUsers.Clear();
lostUsers = null;
}
//sbDebug.Append("\n\n");
//if (!string.IsNullOrEmpty(logFileName))
// System.IO.File.AppendAllText(logFileName, sbDebug.ToString());
//sbDebug.Clear();
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;
}
/// <summary>
/// Returns the sensor-specific userId, given the merged userId.
/// </summary>
/// <param name="sensorIndex">Sensor index</param>
/// <param name="userId">Merged user ID</param>
/// <returns>Sensor-specific user ID</returns>
public ulong GetSensorTrackingId(int sensorIndex, ulong userId)
{
if (adUserIdToSensorTrackingId == null || adUserIdToSensorTrackingId.Length < sensorIndex)
return 0;
if (adUserIdToSensorTrackingId[sensorIndex].ContainsKey(userId))
{
return adUserIdToSensorTrackingId[sensorIndex][userId];
}
return 0;
}
// finds all other overlapping bodies in the list
private void FindOverlappingBodies(List<KinectInterop.BodyData> alCloseBodies, List<KinectInterop.BodyData> alAllBodies,
float mergeDistance, int mBodyIndex)
{
Vector3 pelvisAvgPos = GetBodyJointAvgPos(alCloseBodies, (int)KinectInterop.JointType.Pelvis);
//Vector3 headAvgPos = GetBodyJointAvgPos(alCloseBodies, (int)KinectInterop.JointType.Head);
string sensorUserId0 = alCloseBodies.Count > 0 ? alCloseBodies[0].sensorIndex.ToString() + "_" + alCloseBodies[0].liTrackingID.ToString() : string.Empty;
ulong firstUserId = dictSensorUserIdToUserId.ContainsKey(sensorUserId0) ? dictSensorUserIdToUserId[sensorUserId0] : 0;
int bodyIndex = 0;
while((bodyIndex = GetClosestBodyIndex(alAllBodies, firstUserId, pelvisAvgPos, mergeDistance, alCloseBodies)) >= 0)
{
alCloseBodies.Add(alAllBodies[bodyIndex]);
alAllBodies.RemoveAt(bodyIndex);
}
//Debug.Log("mBodyIndex " + mBodyIndex + " has " + alCloseBodies.Count + " mergeable bodies.");
}
// returns the index of the closest body
private int GetClosestBodyIndex(List<KinectInterop.BodyData> alAllBodies, ulong userId, Vector3 pelvisPos, float maxDistance,
List<KinectInterop.BodyData> alCloseBodies)
{
int bodyIndex = -1;
float minDistance2 = float.MaxValue;
float maxDistance2 = maxDistance * maxDistance;
int bodyCount = alAllBodies.Count;
for (int i = 0; i < bodyCount; i++)
{
int sensorIndex = alAllBodies[i].sensorIndex;
string sensorUserId = sensorIndex.ToString() + "_" + alAllBodies[i].liTrackingID.ToString();
ulong curUserId = dictSensorUserIdToUserId.ContainsKey(sensorUserId) ? dictSensorUserIdToUserId[sensorUserId] : 0;
Vector3 pelvisUserPos = alAllBodies[i].joint[(int)KinectInterop.JointType.Pelvis].position;
float pelvisDist2 = Vector3.SqrMagnitude(pelvisUserPos - pelvisPos);
if (((userId != 0 && curUserId == userId) || pelvisDist2 <= maxDistance2) && pelvisDist2 < minDistance2 &&
!IsBodyListContainsSensorIndex(alCloseBodies, sensorIndex)) // prevent sensor-body duplications (more than one body from the same sensor in the list)
{
bodyIndex = i;
minDistance2 = pelvisDist2;
}
}
return bodyIndex;
}
// checks whether the list of body data already contains body with the specified sensor-index, or not
private bool IsBodyListContainsSensorIndex(List<KinectInterop.BodyData> alBodies, int sensorIndex)
{
foreach(KinectInterop.BodyData body in alBodies)
{
if (body.sensorIndex == sensorIndex)
return true;
}
return false;
}
// returns averaged position of a body joint in a list of bodies
private Vector3 GetBodyJointAvgPos(List<KinectInterop.BodyData> alBodyList, int jointIndex)
{
Vector3 avgJointPos = Vector3.zero;
int bodyCount = alBodyList.Count;
int jointPosCount = 0;
for (int i = 0; i < alBodyList.Count; i++)
{
if((int)alBodyList[i].joint[jointIndex].trackingState >= (int)KinectInterop.TrackingState.Tracked)
{
avgJointPos += alBodyList[i].joint[jointIndex].position;
jointPosCount++;
}
}
avgJointPos = jointPosCount > 0 ? avgJointPos / jointPosCount : Vector3.zero;
return avgJointPos;
}
// averages the bodies in the list and returns the single merged body
private KinectInterop.BodyData GetMergedBody(List<KinectInterop.BodyData> alCloseBodies, int bodyIndex, ref Matrix4x4 world2SensorMat,
List<string> lostUsers, List<KinectInterop.BodyData> alMergedBodies)
{
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);
int minTrackingState = GetBodyJointMinState(alCloseBodies, j);
CalcAverageBodyJoint(alCloseBodies, j, minTrackingState, ref world2SensorMat, ref mergedBody);
}
mergedBody.liTrackingID = GetMergedBodyId(alCloseBodies, lostUsers, alMergedBodies);
mergedBody.iBodyIndex = bodyIndex;
for(int i = 0; i < alCloseBodies.Count; i++)
{
adUserIdToSensorTrackingId[alCloseBodies[i].sensorIndex][mergedBody.liTrackingID] = alCloseBodies[i].liTrackingID;
}
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;
}
// returns max tracking state of a body joint in a list of bodies
private int GetBodyJointMaxState(List<KinectInterop.BodyData> alBodyList, int jointIndex)
{
int maxState = (int)KinectInterop.TrackingState.NotTracked;
int bodyCount = alBodyList.Count;
for (int i = 0; i < bodyCount; i++)
{
if ((int)alBodyList[i].joint[jointIndex].trackingState > maxState)
{
maxState = (int)alBodyList[i].joint[jointIndex].trackingState;
}
}
return maxState;
}
// returns min tracking state of a body joint in a list of bodies
private int GetBodyJointMinState(List<KinectInterop.BodyData> alBodyList, int jointIndex)
{
int minState = (int)KinectInterop.TrackingState.HighConf;
int bodyCount = alBodyList.Count;
for (int i = 0; i < bodyCount; i++)
{
if ((int)alBodyList[i].joint[jointIndex].trackingState < minState)
{
minState = (int)alBodyList[i].joint[jointIndex].trackingState;
}
}
return minState;
}
// whether the joint is
private static readonly bool[] isSingleSensorJoint =
{
false, // Pelvis
false, // SpineNaval
false, // SpineChest
false, // Neck
false, // Head
false, // ClavicleLeft
false, // ShoulderLeft
false, // ElbowLeft
true, // WristLeft
true, // HandLeft
false, // ClavicleRight
false, // ShoulderRight
false, // ElbowRight
true, // WristRight
true, // HandRight
false, // HipLeft
false, // KneeLeft
true, // AnkleLeft
true, // FootLeft
false, // HipRight
false, // KneeRight
true, // AnkleRight
true, // FootRight
true, // Nose
true, // EyeLeft
true, // EarLeft
true, // EyeRight
true, // EarRight
true, // HandtipLeft
true, // ThumbLeft
true, // HandtipRight
true // ThumbRight
};
// returns averaged position of a body joint in a list of bodies
private void CalcAverageBodyJoint(List<KinectInterop.BodyData> alBodyList, int jointIndex, int minTrackingState, ref Matrix4x4 world2SensorMat,
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;
for (int i = 0; i < bodyCount; i++)
{
//if (SINGLE_SENSOR_INDEX >= 0 && alBodyList[i].sensorIndex != SINGLE_SENSOR_INDEX)
// continue;
//if (jointIndex == (int)KinectInterop.JointType.WristRight)
//{
// Debug.Log(string.Format("BM {0:F3} {1}: {2}_{3}, state: {4}\npos: {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 (jointIndex == (int)KinectInterop.JointType.WristLeft)
//{
// Debug.Log(string.Format("BM {0:F3} {1}: {2}_{3}, state: {4}\npos: {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));
//}
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 (isSingleSensorJoint[jointIndex])
{
//minTrackingState = (int)jointState;
//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);
}
// avg kinect pos
Vector3 avgKinectPos = world2SensorMat.MultiplyPoint3x4(avgJointPos);
Vector3 spaceScale = firstSensorData != null ? firstSensorData.sensorSpaceScale : Vector3.one;
avgKinectPos = new Vector3(avgKinectPos.x * spaceScale.x, avgKinectPos.y * spaceScale.y, avgKinectPos.z * spaceScale.z);
// set joint data
KinectInterop.JointData jointData = bodyData.joint[jointIndex];
jointData.trackingState = (KinectInterop.TrackingState)minTrackingState;
jointData.kinectPos = avgKinectPos; // 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);
//if (jointIndex == (int)KinectInterop.JointType.WristRight)
//{
// Debug.Log(string.Format("BM1 {0}: pos: {1}, rot: {2}\n", (KinectInterop.JointType)jointIndex, jointData.position, jointData.mirroredRotation.eulerAngles));
//}
bodyData.joint[jointIndex] = jointData;
}
// averages the bodies in the list and returns the single merged body
private ulong GetMergedBodyId(List<KinectInterop.BodyData> alCloseBodies, List<string> lostUsers,
List<KinectInterop.BodyData> alMergedBodies)
{
int bodyCount = alCloseBodies.Count;
float minStartTime = float.MaxValue;
ulong userBodyId = 0;
// look for the oldest available user id
for (int i = 0; i < bodyCount; i++)
{
string sensorUserId = alCloseBodies[i].sensorIndex.ToString() + "_" + alCloseBodies[i].liTrackingID.ToString();
if(dictSensorUserIdToUserId.ContainsKey(sensorUserId) && dictSensorUserIdToFirstUsed[sensorUserId] < minStartTime &&
!IsBodyListContainsUserId(alMergedBodies, dictSensorUserIdToUserId[sensorUserId])) // prevent userId duplications in merged-body list
{
userBodyId = dictSensorUserIdToUserId[sensorUserId];
minStartTime = dictSensorUserIdToFirstUsed[sensorUserId];
}
}
for (int i = 0; i < bodyCount; i++)
{
string sensorUserId = alCloseBodies[i].sensorIndex.ToString() + "_" + alCloseBodies[i].liTrackingID.ToString();
if (userBodyId == 0)
{
if (!dictSensorUserIdToUserId.ContainsKey(sensorUserId) ||
IsBodyListContainsUserId(alMergedBodies, dictSensorUserIdToUserId[sensorUserId])) // prevent userId duplications in merged-body list
{
//Debug.Log("Creating new userId '" + nextUserId + "' for sensor-user-id '" + sensorUserId + "'");
dictSensorUserIdToUserId[sensorUserId] = nextUserId;
dictSensorUserIdToFirstUsed[sensorUserId] = Time.time;
dictSensorUserIdToLastUsed[sensorUserId] = Time.time;
nextUserId++;
}
userBodyId = dictSensorUserIdToUserId[sensorUserId];
}
else if (!dictSensorUserIdToUserId.ContainsKey(sensorUserId) || dictSensorUserIdToUserId[sensorUserId] != userBodyId)
{
//ulong oldUserId = dictSensorUserIdToUserId.ContainsKey(sensorUserId) ? dictSensorUserIdToUserId[sensorUserId] : 0;
//Debug.Log("Updating userId for sensor-user-id '" + sensorUserId + "' from '" + oldUserId + "' to '" + userBodyId + "'");
dictSensorUserIdToUserId[sensorUserId] = userBodyId;
if (!dictSensorUserIdToFirstUsed.ContainsKey(sensorUserId))
dictSensorUserIdToFirstUsed[sensorUserId] = Time.time;
dictSensorUserIdToLastUsed[sensorUserId] = Time.time;
}
if (lostUsers.Contains(sensorUserId))
{
lostUsers.Remove(sensorUserId);
}
}
return userBodyId;
}
// checks whether the list of body data already contains body with the specified userId, or not
private bool IsBodyListContainsUserId(List<KinectInterop.BodyData> alBodies, ulong userId)
{
foreach (KinectInterop.BodyData body in alBodies)
{
if (body.liTrackingID == userId)
return true;
}
return false;
}
}
}