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