/// <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)); } } }