public static KinectBodies ReadSkeletonBinFile(string filename) { var bodies = new KinectBodies(); if (File.Exists(filename)) { using (var memStream = new MemoryStream(File.ReadAllBytes(filename))) { bodies = ProtoBuf.Serializer.Deserialize<KinectBodies>(memStream); } } return bodies; }
private void PublishKinectBodies(KinectBodies kbodies, bool force = true) { if (kbodies != null && _socket != null) //if (kbodies != null && _socket != null) { if (force || kbodies.Body.Count > 0) { _socket.SendMore(_topic); using (var ms = new MemoryStream()) { Serializer.Serialize(ms, kbodies); _socket.Send(ms.GetBuffer(), (int)ms.Length); } } } }
/// <summary> /// Update the skeleton information of the humans /// </summary> /// <param name="kinectBodies">Skeleton list</param> public void Update(KinectBodies kinectBodies) { lock (_object) { if (kinectBodies.Body.Count <= 0) { Humans.Clear(); return; } var keys = Humans.Select(s => s.Body.TrackingId).ToList(); var bodyIds = kinectBodies.Body.Select(s => s.TrackingId).ToList(); // Remove those humans that dont exist in the list of kinect bodies anymore foreach (var key in keys) { if (!bodyIds.Contains(key)) { var item = Humans.FirstOrDefault(s => s.Body.TrackingId == key); if (item != null) { Humans.Remove(item); Log.InfoFormat("Removed human: {0}", item.Id); } } } foreach (var kinectBody in kinectBodies.Body) { var item = Humans.FirstOrDefault(s => s.Body.TrackingId == kinectBody.TrackingId); if (item == null) { var human = new Human(kinectBody.TrackingId, _motionModules) { Body = kinectBody }; Humans.Add(human); Log.InfoFormat("Added new human: {0}", human.Id); } else { item.Body = kinectBody; } } if (Humans.Count > 0 && !_serializeHuman) { var humansStr = JsonConvert.SerializeObject(Humans); var humansObj = JsonConvert.DeserializeObject<ObservableCollection<Human>>(humansStr); if (humansObj != null) { Console.WriteLine(humansStr); } _serializeHuman = true; } } }
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++; //} }