public Point MapJointToDepth(KinectBase.Joint joint, bool undoTransform) { //TODO: Update this so it takes a joint array instead of a single joint (this is supposed to be more efficient for the Kinect 2) Point mappedPoint = new Point(0, 0); Point3D transformedPosition = joint.Position; if (undoTransform) { Matrix3D inverseTransform = skeletonTransformation; inverseTransform.Invert(); transformedPosition = inverseTransform.Transform(transformedPosition); } //Setup the Kinect v2 objects to do the transformation CameraSpacePoint[] skelPoints = new CameraSpacePoint[1]; skelPoints[0] = new CameraSpacePoint(); skelPoints[0].X = (float)transformedPosition.X; skelPoints[0].Y = (float)transformedPosition.Y; skelPoints[0].Z = (float)transformedPosition.Z; DepthSpacePoint[] points = new DepthSpacePoint[1]; kinect.CoordinateMapper.MapCameraPointsToDepthSpace(skelPoints, points); //Convert back to the base object type mappedPoint.X = points[0].X; mappedPoint.Y = points[0].Y; return(mappedPoint); }
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.Joint TransformJoint(KinectBase.Joint joint) { KinectBase.Joint transformedJoint = new KinectBase.Joint(); transformedJoint.Confidence = joint.Confidence; transformedJoint.JointType = joint.JointType; transformedJoint.TrackingState = joint.TrackingState; transformedJoint.Orientation.orientationQuaternion = skeletonRotQuaternion * joint.Orientation.orientationQuaternion; transformedJoint.Position = skeletonTransformation.Transform(joint.Position); transformedJoint.utcTime = joint.utcTime; return(transformedJoint); }
public Point MapJointToDepth(KinectBase.Joint joint, bool undoTransform) { Point mappedPoint = new Point(0, 0); Point3D transformedPosition = joint.Position; if (undoTransform) { Matrix3D inverseTransform = skeletonTransformation; inverseTransform.Invert(); transformedPosition = inverseTransform.Transform(transformedPosition); } SkeletonPoint skelPoint = new SkeletonPoint(); skelPoint.X = (float)transformedPosition.X; skelPoint.Y = (float)transformedPosition.Y; skelPoint.Z = (float)transformedPosition.Z; DepthImagePoint point = kinect.CoordinateMapper.MapSkeletonPointToDepthPoint(skelPoint, kinect.DepthStream.Format); mappedPoint.X = point.X; mappedPoint.Y = point.Y; return(mappedPoint); }
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.Joint TransformJoint(KinectBase.Joint joint) { KinectBase.Joint transformedJoint = new KinectBase.Joint(); transformedJoint.Confidence = joint.Confidence; transformedJoint.JointType = joint.JointType; transformedJoint.TrackingState = joint.TrackingState; transformedJoint.Orientation = skeletonRotQuaternion * joint.Orientation; transformedJoint.Position = skeletonTransformation.Transform(joint.Position); transformedJoint.utcTime = joint.utcTime; return transformedJoint; }
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); } } }