/// <summary>
/// Similar to the Kinect SDK method of the same name, this method retrieves
/// the array of bodies from a <c>BodyFrame</c> and updates the specified
/// <c>SmoothedBodyList</c> collection. If the collection does not contain
/// the correct number of bodies, this method clears and refills the collection
/// with new bodies of type T. Note that if this behavior is undesirable,
/// insure that the collection contains the right number of bodies before
/// calling this method.
/// </summary>
/// <param name="frame">The frame.</param>
/// <param name="bodies">The bodies.</param>
/// <exception cref="System.ArgumentNullException">bodies list must not be null</exception>
public static void GetAndRefreshBodyData(this BodyFrame frame, SmoothedBodyList <ISmoother> bodies)
{
if (bodies == null)
{
throw new ArgumentNullException("bodies list must not be null");
}
if (_bodies == null || frame.BodyCount != _bodies.Length)
{
_bodies = new Body[frame.BodyCount];
}
frame.GetAndRefreshBodyData(_bodies);
bodies.RefreshFromBodyArray(_bodies);
}
/// <summary>
/// <para>
/// Similar to the Kinect SDK method of the same name, this method updates the
/// specified <c>SmoothedBodyList</c> collection with the values contained in the
/// Bodies list of this <c>ReplayBodyFrame</c>. If the collection does not contain
/// the correct number of bodies, this method clears and refills the collection
/// with new bodies of type T. Note that if this behavior is undesirable,
/// insure that the collection contains the right number of bodies before
/// calling this method.
/// </para>
/// <para>
/// Note that this method is really only needed if you wish to use smoothing
/// during replay. Otherwise, directly accessing the Bodies list of this
/// <c>ReplayBodyFrame</c> is both acceptable and more efficient.
/// </para>
/// </summary>
public void GetAndRefreshBodyData <T>(SmoothedBodyList <T> bodies) where T : ISmoother
{
if (bodies == null)
{
throw new ArgumentNullException("bodies list must not be null");
}
if (this.BodyCount != bodies.Count)
{
bodies.Fill(this.BodyCount);
}
for (var i = 0; i < this.BodyCount; i++)
{
bodies[i].Update(this.Bodies[i]);
}
}
void SmoothingCombo_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
_bodies = new List <CustomBody>();
_kalmanBodies = new SmoothedBodyList <KalmanSmoother>();
_exponentialBodies = new SmoothedBodyList <ExponentialSmoother>();
}
void SmoothingCombo_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
_bodies = new List<CustomBody>();
_kalmanBodies = new SmoothedBodyList<KalmanSmoother>();
_exponentialBodies = new SmoothedBodyList<ExponentialSmoother>();
}
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++;
//}
}
public void UpdateBodyFrame(SmoothedBodyList<ExponentialSmoother> bodies)
#endif
{
if (bodies != null)
{
using (DrawingContext dc = _drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, _displayWidth, _displayHeight));
int penIndex = 0;
foreach (var body in bodies)
{
Pen drawPen = KinectBodyHelper.Instance.BodyColors[penIndex++];
if (body.IsTracked)
{
DrawClippedEdges(body, dc);
var joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
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;
}
DepthSpacePoint depthSpacePoint = _coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
DrawBody(joints, jointPoints, dc, drawPen);
DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
}
}
// prevent drawing outside of our render area
_drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, _displayWidth, _displayHeight));
}
}
}
