void skeletonReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (BodyFrame skelFrame = e.FrameReference.AcquireFrame())
{
if (skelFrame != null)
{
Body[] skeletons = new Body[skelFrame.BodyCount];
skelFrame.GetAndRefreshBodyData(skeletons);
DateTime now = DateTime.UtcNow;
//Convert from Kinect v2 skeletons to KVR skeletons
KinectBase.KinectSkeleton[] kvrSkeletons = new KinectBase.KinectSkeleton[skelFrame.BodyCount];
for (int i = 0; i < skelFrame.BodyCount; i++)
{
kvrSkeletons[i] = new KinectBase.KinectSkeleton();
kvrSkeletons[i].Position = new Point3D(skeletons[i].Joints[JointType.SpineBase].Position.X, skeletons[i].Joints[JointType.SpineBase].Position.Y, skeletons[i].Joints[JointType.SpineBase].Position.Z);
kvrSkeletons[i].SkeletonTrackingState = convertTrackingState(skeletons[i].IsTracked);
kvrSkeletons[i].TrackingId = (int)skeletons[i].TrackingId;
//kvrSkeletons[i].utcSampleTime = DateTime.UtcNow;
kvrSkeletons[i].sourceKinectID = kinectID;
for (int j = 0; j < Body.JointCount; j++)
{
KinectBase.Joint newJoint = new KinectBase.Joint();
newJoint.JointType = convertJointType(skeletons[i].Joints[(JointType)j].JointType);
newJoint.Position = convertJointPosition(skeletons[i].Joints[(JointType)j].Position);
newJoint.TrackingState = convertTrackingState(skeletons[i].Joints[(JointType)j].TrackingState);
newJoint.Orientation = convertJointOrientation(skeletons[i].JointOrientations[(JointType)j].Orientation);
newJoint.utcTime = now;
//Tracking confidence only exists for the hand states, so set those and leave the rest as unknown
if (newJoint.JointType == KinectBase.JointType.HandLeft)
{
newJoint.Confidence = convertTrackingConfidence(skeletons[i].HandLeftConfidence);
}
else if (newJoint.JointType == KinectBase.JointType.HandRight)
{
newJoint.Confidence = convertTrackingConfidence(skeletons[i].HandRightConfidence);
}
else
{
newJoint.Confidence = KinectBase.TrackingConfidence.Unknown;
}
kvrSkeletons[i].skeleton[newJoint.JointType] = newJoint;
}
kvrSkeletons[i].rightHandClosed = convertHandState(skeletons[i].HandRightState);
kvrSkeletons[i].leftHandClosed = convertHandState(skeletons[i].HandLeftState);
}
//Add the skeleton data to the event handler and throw the event
KinectBase.SkeletonEventArgs skelE = new KinectBase.SkeletonEventArgs();
skelE.skeletons = kvrSkeletons;
skelE.kinectID = kinectID;
OnSkeletonChanged(skelE);
}
}
}
public KinectBase.KinectSkeleton TransformSkeleton(KinectBase.KinectSkeleton skeleton)
{
KinectBase.KinectSkeleton transformedSkeleton = new KinectBase.KinectSkeleton();
transformedSkeleton.leftHandClosed = skeleton.leftHandClosed;
transformedSkeleton.rightHandClosed = skeleton.rightHandClosed;
transformedSkeleton.TrackingId = skeleton.TrackingId;
transformedSkeleton.SkeletonTrackingState = skeleton.SkeletonTrackingState;
//transformedSkeleton.utcSampleTime = skeleton.utcSampleTime;
transformedSkeleton.sourceKinectID = skeleton.sourceKinectID;
transformedSkeleton.Position = skeletonTransformation.Transform(skeleton.Position);
//Transform the joints
for (int i = 0; i < skeleton.skeleton.Count; i++)
{
transformedSkeleton.skeleton[i] = TransformJoint(skeleton.skeleton[i]);
}
return(transformedSkeleton);
}
private void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame skelFrame = e.OpenSkeletonFrame())
{
if (skelFrame != null && masterSettings.kinectOptionsList.Count > kinectID && (masterKinectSettings.mergeSkeletons || masterKinectSettings.sendRawSkeletons))
{
DateTime now = DateTime.UtcNow;
Skeleton[] skeletons = new Skeleton[skelFrame.SkeletonArrayLength];
skelFrame.CopySkeletonDataTo(skeletons);
EigenWrapper.Matrix predAccel = new EigenWrapper.Matrix(3, 1);
predAccel[2, 0] = 1;
if (accelFilterStarted)
{
predAccel = accelerationFilter.PredictAndDiscard(0);
}
if (interactStream != null)
{
Vector4 filteredAccel = new Vector4();
filteredAccel.W = 0;
filteredAccel.X = (float)predAccel[0, 0];
filteredAccel.Y = (float)predAccel[1, 0];
filteredAccel.Z = (float)predAccel[2, 0];
interactStream.ProcessSkeleton(skeletons, filteredAccel, skelFrame.Timestamp);
//System.Diagnostics.Trace.WriteLine("[" + filteredAccel.X + ", " + filteredAccel.Y + ", " + filteredAccel.Z + "]");
}
//Generate the transformation matrix for the skeletons
double kinectYaw = masterKinectSettings.kinectYaw;
Point3D kinectPosition = masterKinectSettings.kinectPosition;
Matrix3D gravityBasedKinectRotation = findRotation(new Vector3D(predAccel[0, 0], predAccel[1, 0], predAccel[2, 0]), new Vector3D(0, -1, 0));
AxisAngleRotation3D yawRotation = new AxisAngleRotation3D(new Vector3D(0, 1, 0), -kinectYaw);
RotateTransform3D tempYawTrans = new RotateTransform3D(yawRotation);
TranslateTransform3D transTrans = new TranslateTransform3D((Vector3D)kinectPosition);
Matrix3D rotationOnlyMatrix = Matrix3D.Multiply(tempYawTrans.Value, gravityBasedKinectRotation);
Matrix3D masterMatrix = Matrix3D.Multiply(rotationOnlyMatrix, transTrans.Value);
skeletonTransformation = masterMatrix;
skeletonRotQuaternion = KinectBase.HelperMethods.MatrixToQuaternion(rotationOnlyMatrix);
//Convert from Kinect v1 skeletons to KVR skeletons
KinectBase.KinectSkeleton[] kvrSkeletons = new KinectBase.KinectSkeleton[skeletons.Length];
for (int i = 0; i < kvrSkeletons.Length; i++)
{
//Set the tracking ID numbers for the hand grab data
int grabID = -1;
for (int j = 0; j < skeletonHandGrabData.Count; j++)
{
if (skeletonHandGrabData[j].skeletonTrackingID == skeletons[i].TrackingId)
{
grabID = j;
break;
}
}
if (grabID < 0)
{
skeletonHandGrabData.Add(new HandGrabInfo(skeletons[i].TrackingId));
grabID = skeletonHandGrabData.Count - 1;
}
kvrSkeletons[i] = new KinectBase.KinectSkeleton();
kvrSkeletons[i].Position = new Point3D(skeletons[i].Position.X, skeletons[i].Position.Y, skeletons[i].Position.Z);
kvrSkeletons[i].SkeletonTrackingState = convertTrackingState(skeletons[i].TrackingState);
kvrSkeletons[i].TrackingId = skeletons[i].TrackingId;
//kvrSkeletons[i].utcSampleTime = DateTime.UtcNow;
kvrSkeletons[i].sourceKinectID = kinectID;
for (int j = 0; j < skeletons[i].Joints.Count; j++)
{
KinectBase.Joint newJoint = new KinectBase.Joint();
newJoint.Confidence = KinectBase.TrackingConfidence.Unknown; //The Kinect 1 doesn't support the confidence property
newJoint.JointType = convertJointType(skeletons[i].Joints[(JointType)j].JointType);
Vector4 tempQuat = skeletons[i].BoneOrientations[(JointType)j].AbsoluteRotation.Quaternion;
newJoint.Orientation.orientationQuaternion = new Quaternion(tempQuat.X, tempQuat.Y, tempQuat.Z, tempQuat.W);
Matrix4 tempMat = skeletons[i].BoneOrientations[(JointType)j].AbsoluteRotation.Matrix;
newJoint.Orientation.orientationMatrix = new Matrix3D(tempMat.M11, tempMat.M12, tempMat.M13, tempMat.M14,
tempMat.M21, tempMat.M22, tempMat.M23, tempMat.M24,
tempMat.M31, tempMat.M32, tempMat.M33, tempMat.M34,
tempMat.M41, tempMat.M42, tempMat.M43, tempMat.M44);
SkeletonPoint tempPos = skeletons[i].Joints[(JointType)j].Position;
newJoint.Position = new Point3D(tempPos.X, tempPos.Y, tempPos.Z);
newJoint.TrackingState = convertTrackingState(skeletons[i].Joints[(JointType)j].TrackingState);
newJoint.spatialErrorStdDev = getJointError(newJoint.Position, newJoint.TrackingState);
newJoint.utcTime = now;
kvrSkeletons[i].skeleton[newJoint.JointType] = newJoint; //Skeleton doesn't need to be initialized because it is done in the KinectSkeleton constructor
}
//Get the hand states from the hand grab data array
kvrSkeletons[i].rightHandClosed = skeletonHandGrabData[grabID].rightHandClosed;
kvrSkeletons[i].leftHandClosed = skeletonHandGrabData[grabID].leftHandClosed;
}
//Add the skeleton data to the event handler and throw the event
KinectBase.SkeletonEventArgs skelE = new KinectBase.SkeletonEventArgs();
skelE.skeletons = kvrSkeletons;
skelE.kinectID = kinectID;
OnSkeletonChanged(skelE);
}
}
}
void skeletonReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (BodyFrame skelFrame = e.FrameReference.AcquireFrame())
{
if (skelFrame != null)
{
Body[] skeletons = new Body[skelFrame.BodyCount];
skelFrame.GetAndRefreshBodyData(skeletons);
DateTime now = DateTime.UtcNow;
//Convert from Kinect v2 skeletons to KVR skeletons
KinectBase.KinectSkeleton[] kvrSkeletons = new KinectBase.KinectSkeleton[skelFrame.BodyCount];
for (int i = 0; i < skelFrame.BodyCount; i++)
{
kvrSkeletons[i] = new KinectBase.KinectSkeleton();
kvrSkeletons[i].Position = new Point3D(skeletons[i].Joints[JointType.SpineBase].Position.X, skeletons[i].Joints[JointType.SpineBase].Position.Y, skeletons[i].Joints[JointType.SpineBase].Position.Z);
kvrSkeletons[i].SkeletonTrackingState = convertTrackingState(skeletons[i].IsTracked);
kvrSkeletons[i].TrackingId = (int)skeletons[i].TrackingId;
//kvrSkeletons[i].utcSampleTime = DateTime.UtcNow;
kvrSkeletons[i].sourceKinectID = kinectID;
for (int j = 0; j < Body.JointCount; j++)
{
KinectBase.Joint newJoint = new KinectBase.Joint();
newJoint.JointType = convertJointType(skeletons[i].Joints[(JointType)j].JointType);
newJoint.Position = convertJointPosition(skeletons[i].Joints[(JointType)j].Position);
newJoint.TrackingState = convertTrackingState(skeletons[i].Joints[(JointType)j].TrackingState);
newJoint.Orientation = convertJointOrientation(skeletons[i].JointOrientations[(JointType)j].Orientation);
newJoint.utcTime = now;
//Tracking confidence only exists for the hand states, so set those and leave the rest as unknown
if (newJoint.JointType == KinectBase.JointType.HandLeft)
{
newJoint.Confidence = convertTrackingConfidence(skeletons[i].HandLeftConfidence);
}
else if (newJoint.JointType == KinectBase.JointType.HandRight)
{
newJoint.Confidence = convertTrackingConfidence(skeletons[i].HandRightConfidence);
}
else
{
newJoint.Confidence = KinectBase.TrackingConfidence.Unknown;
}
kvrSkeletons[i].skeleton[newJoint.JointType] = newJoint;
}
kvrSkeletons[i].rightHandClosed = convertHandState(skeletons[i].HandRightState);
kvrSkeletons[i].leftHandClosed = convertHandState(skeletons[i].HandLeftState);
}
//Add the skeleton data to the event handler and throw the event
KinectBase.SkeletonEventArgs skelE = new KinectBase.SkeletonEventArgs();
skelE.skeletons = kvrSkeletons;
skelE.kinectID = kinectID;
OnSkeletonChanged(skelE);
}
}
}
public KinectBase.KinectSkeleton TransformSkeleton(KinectBase.KinectSkeleton skeleton)
{
KinectBase.KinectSkeleton transformedSkeleton = new KinectBase.KinectSkeleton();
transformedSkeleton.leftHandClosed = skeleton.leftHandClosed;
transformedSkeleton.rightHandClosed = skeleton.rightHandClosed;
transformedSkeleton.TrackingId = skeleton.TrackingId;
transformedSkeleton.SkeletonTrackingState = skeleton.SkeletonTrackingState;
//transformedSkeleton.utcSampleTime = skeleton.utcSampleTime;
transformedSkeleton.sourceKinectID = skeleton.sourceKinectID;
transformedSkeleton.Position = skeletonTransformation.Transform(skeleton.Position);
//Transform the joints
for (int i = 0; i < skeleton.skeleton.Count; i++)
{
transformedSkeleton.skeleton[i] = TransformJoint(skeleton.skeleton[i]);
}
return transformedSkeleton;
}
private void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame skelFrame = e.OpenSkeletonFrame())
{
if (skelFrame != null && masterSettings.kinectOptionsList.Count > kinectID && (masterKinectSettings.mergeSkeletons || masterKinectSettings.sendRawSkeletons))
{
DateTime now = DateTime.UtcNow;
Skeleton[] skeletons = new Skeleton[skelFrame.SkeletonArrayLength];
skelFrame.CopySkeletonDataTo(skeletons);
if (interactStream != null && lastAcceleration != null)
{
interactStream.ProcessSkeleton(skeletons, lastAcceleration, skelFrame.Timestamp);
}
//Generate the transformation matrix for the skeletons
double kinectYaw = masterKinectSettings.kinectYaw;
Point3D kinectPosition = masterKinectSettings.kinectPosition;
Matrix3D gravityBasedKinectRotation = findRotation(new Vector3D(lastAcceleration.X, lastAcceleration.Y, lastAcceleration.Z), new Vector3D(0, -1, 0));
AxisAngleRotation3D yawRotation = new AxisAngleRotation3D(new Vector3D(0, 1, 0), -kinectYaw);
RotateTransform3D tempTrans = new RotateTransform3D(yawRotation);
TranslateTransform3D transTrans = new TranslateTransform3D((Vector3D)kinectPosition);
Matrix3D masterMatrix = Matrix3D.Multiply(Matrix3D.Multiply(tempTrans.Value, gravityBasedKinectRotation), transTrans.Value);
skeletonTransformation = masterMatrix;
//Convert from Kinect v1 skeletons to KVR skeletons
KinectBase.KinectSkeleton[] kvrSkeletons = new KinectBase.KinectSkeleton[skeletons.Length];
for (int i = 0; i < kvrSkeletons.Length; i++)
{
//Set the tracking ID numbers for the hand grab data
int grabID = -1;
for (int j = 0; j < skeletonHandGrabData.Count; j++)
{
if (skeletonHandGrabData[j].skeletonTrackingID == skeletons[i].TrackingId)
{
grabID = j;
break;
}
}
if (grabID < 0)
{
skeletonHandGrabData.Add(new HandGrabInfo(skeletons[i].TrackingId));
grabID = skeletonHandGrabData.Count - 1;
}
kvrSkeletons[i] = new KinectBase.KinectSkeleton();
kvrSkeletons[i].Position = new Point3D(skeletons[i].Position.X, skeletons[i].Position.Y, skeletons[i].Position.Z);
kvrSkeletons[i].SkeletonTrackingState = convertTrackingState(skeletons[i].TrackingState);
kvrSkeletons[i].TrackingId = skeletons[i].TrackingId;
//kvrSkeletons[i].utcSampleTime = DateTime.UtcNow;
kvrSkeletons[i].sourceKinectID = kinectID;
for (int j = 0; j < skeletons[i].Joints.Count; j++)
{
KinectBase.Joint newJoint = new KinectBase.Joint();
newJoint.Confidence = KinectBase.TrackingConfidence.Unknown; //The Kinect 1 doesn't support the confidence property
newJoint.JointType = convertJointType(skeletons[i].Joints[(JointType)j].JointType);
Vector4 tempQuat = skeletons[i].BoneOrientations[(JointType)j].AbsoluteRotation.Quaternion;
newJoint.Orientation = new Quaternion(tempQuat.X, tempQuat.Y, tempQuat.Z, tempQuat.W);
SkeletonPoint tempPos = skeletons[i].Joints[(JointType)j].Position;
newJoint.Position = new Point3D(tempPos.X, tempPos.Y, tempPos.Z);
newJoint.TrackingState = convertTrackingState(skeletons[i].Joints[(JointType)j].TrackingState);
newJoint.utcTime = now;
kvrSkeletons[i].skeleton[newJoint.JointType] = newJoint; //Skeleton doesn't need to be initialized because it is done in the KinectSkeleton constructor
}
//Get the hand states from the hand grab data array
kvrSkeletons[i].rightHandClosed = skeletonHandGrabData[grabID].rightHandClosed;
kvrSkeletons[i].leftHandClosed = skeletonHandGrabData[grabID].leftHandClosed;
}
//Add the skeleton data to the event handler and throw the event
KinectBase.SkeletonEventArgs skelE = new KinectBase.SkeletonEventArgs();
skelE.skeletons = kvrSkeletons;
skelE.kinectID = kinectID;
OnSkeletonChanged(skelE);
}
}
}