/// <summary>
/// Update humans information
/// </summary>
/// <param name="humans">List of human</param>
public void Update(Humans humans)
{
if (humans != null)
{
lock (_object)
{
foreach (var human in humans.human)
{
//TODO Need to unify the usage of tracking id across the application
var item = _humans.FirstOrDefault(s => s.Body.TrackingId == human.id);
if (item != null)
{
item.HeadPosition = human.head_position;
item.TorsoPosition = human.torso_position;
item.TorsoOrientation = human.orientation;
}
}
}
}
}
public void UpdateBodyFrame(SmoothedBodyList<ExponentialSmoother> bodies, HumanPosePublisher humanPose, NaoJointPublisher naoJoints)
#endif
{
if (!CanSend) return;
KinectBodies kbodies = new KinectBodies();
ParamList list = null;
var humans = new Humans();
//_noBodyTracked = true;
int penIndex = 0;
foreach (var body in bodies)
{
penIndex++;
var human = new Human {tracked = body.IsTracked};
var jointPosDict = new Dictionary<JointType, Vector3D>();
human.id = penIndex-1;
if (body.IsTracked)
{
//_noBodyTracked = false;
//_emptyMsgSent = 0;
if (naoJoints != null)
{
list = naoJoints.GetJointValues(body);
if (list.param.Count > 0)
{
naoJoints.Update(list);
}
}
var kbody = new KinectBody
{
IsTracked = true,
TrackingId = penIndex - 1,
JointCount = body.Joints.Count
};
var joints = body.Joints;
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = KinectBodyHelper.InferredZPositionClamp;
}
var vector3 = new Vector3d {x = position.X, y = position.Y, z = position.Z};
KinectJoint kjoint = new KinectJoint
{
Type = (KinectJoint.JointType) jointType,
State = (KinectJoint.TrackingState) joints[jointType].TrackingState,
Position = vector3
};
var orient = body.JointOrientations[jointType].Orientation;
kjoint.Orientation = new Quaternion
{
w = orient.W,
x = orient.X,
y = orient.Y,
z = orient.Z
};
#if USE_KINECT_BODIES
#else
kjoint.Angle = (float) body.GetAngleAt(jointType);
#endif
kbody.Joints.Add(kjoint);
if (jointType == JointType.SpineBase || jointType == JointType.ShoulderLeft ||
jointType == JointType.ShoulderRight || jointType == JointType.Head ||
jointType == JointType.SpineShoulder)
{
if (jointType == JointType.Head)
{
human.head_position = vector3;
}
if (jointType == JointType.SpineBase)
{
human.torso_position = vector3;
}
jointPosDict.Add(jointType, new Vector3D(position.X, position.Y, position.Z));
}
}
if (jointPosDict.ContainsKey(JointType.SpineBase) &&
jointPosDict.ContainsKey(JointType.ShoulderLeft) &&
jointPosDict.ContainsKey(JointType.ShoulderRight) && jointPosDict.ContainsKey(JointType.Head) &&
jointPosDict.ContainsKey(JointType.SpineShoulder))
{
#if USE_Z_FRONT
var tri = new Triangle(jointPosDict[JointType.SpineBase], jointPosDict[JointType.ShoulderLeft],
jointPosDict[JointType.ShoulderRight]);
var zAxis = tri.Normal(); // Z Axis
var midPt = jointPosDict[JointType.SpineShoulder];
var yAxis = (midPt - jointPosDict[JointType.SpineBase]);
yAxis.Normalize();
var xAxis = Vector3D.CrossProduct(yAxis, zAxis);
xAxis.Normalize();
human.orientation = ToQuaternion(xAxis, yAxis, zAxis);
#else
var tri = new Triangle(jointPosDict[JointType.SpineBase], jointPosDict[JointType.ShoulderLeft],
jointPosDict[JointType.ShoulderRight]);
var xAxis = tri.Normal(); // Z Axis
var midPt = jointPosDict[JointType.SpineShoulder];
var zAxis = (midPt - jointPosDict[JointType.SpineBase]);
zAxis.Normalize();
var yAxis = Vector3D.CrossProduct(zAxis, xAxis);
yAxis.Normalize();
//human.orientation = ToQuaternion(xAxis, yAxis, zAxis);
//var spineBase = jointPosDict[JointType.SpineBase];
//var tri = new Triangle(spineBase, jointPosDict[JointType.ShoulderLeft],
// jointPosDict[JointType.ShoulderRight]);
//var xAxis = tri.Normal(); // Z Axis
//var zAxis = spineBase + new Vector3D(spineBase.X, spineBase.Y, spineBase.Z + 0.5);
//zAxis.Normalize();
//var yAxis = Vector3D.CrossProduct(zAxis, xAxis);
//yAxis.Normalize();
human.orientation = ToQuaternion(xAxis, yAxis, zAxis);
#endif
}
else
{
human.orientation = new Quaternion {w = 1, x = 0, y = 0, z = 0};
}
kbodies.Body.Add(kbody);
humans.human.Add(human);
}
if (humanPose != null)
{
humanPose.Update(humans);
}
//PublishKinectBodies(kbodies);
}
PublishKinectBodies(kbodies);
//if (_noBodyTracked && _emptyMsgSent < 5)
//{
// PublishKinectBodies(kbodies, true);
// _emptyMsgSent++;
//}
}