public OscMessage BuildJointMessage(Skeleton body, Joint joint) { string jointName = joint.JointType.ToString(); if (jointName.IndexOf("Left") >= 0) { jointName = jointName.Replace("Left", "Right"); } else if (jointName.IndexOf("Right") >= 0) { jointName = jointName.Replace("Right", "Left"); } var jointRotation = body.BoneOrientations[joint.JointType].AbsoluteRotation.Matrix; var position = body.Joints[joint.JointType].Position; var rotation = body.BoneOrientations[joint.JointType].AbsoluteRotation.Matrix; var qrotation = body.BoneOrientations[joint.JointType].AbsoluteRotation.Quaternion; Quaternion qOrientation = new Quaternion(qrotation.X, qrotation.Y, qrotation.Z, qrotation.W); Matrix4x4 mjointRot = new Matrix4x4(jointRotation.M11, jointRotation.M12, jointRotation.M13, jointRotation.M14, jointRotation.M21, jointRotation.M22, jointRotation.M23, jointRotation.M24, jointRotation.M31, jointRotation.M32, jointRotation.M33, jointRotation.M34, jointRotation.M41, jointRotation.M42, jointRotation.M43, jointRotation.M44); //System.Numerics.Vector4 v = new System.Numerics.Vector4(qrotation.X, qrotation.Y, qrotation.Z, qrotation.W); //Plane pln = new Plane(v); //Matrix4x4 reflectionParent = System.Numerics.Matrix4x4.CreateReflection(pln); //qSend = Quaternion.CreateFromRotationMatrix(reflectionParent); //qSend.W = qrotation.W; var flipMat = new Matrix4x4(1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); mjointRot = flipMat * mjointRot * flipMat; var address = String.Format("/{0}", joint.JointType); if (KinectHelper.BoneOrientationIsValid(qOrientation)) { Quaternion qSend; qSend = Quaternion.CreateFromRotationMatrix(mjointRot); return(new OscMessage(address, (body.Position.X + position.X), (body.Position.Y + position.Y), (body.Position.Z + position.Z), qSend.W, qSend.X, qSend.Y, qSend.Z)); } return(new OscMessage(address, (body.Position.X + position.X), (body.Position.Y + position.Y), (body.Position.Z + position.Z))); }
// Update the filter for one joint. protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters, ref Matrix4x4[] jointOrientations) { // if (null == skeleton) // { // return; // } // int jointIndex = (int)jt; Quaternion filteredOrientation; Quaternion trend; Vector3 fwdVector = (Vector3)jointOrientations[jointIndex].GetColumn(2); if (fwdVector == Vector3.zero) { return; } Quaternion rawOrientation = Quaternion.LookRotation(fwdVector, jointOrientations[jointIndex].GetColumn(1)); Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation; Quaternion prevTrend = this.history[jointIndex].Trend; Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex]; bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jointIndex) && KinectHelper.BoneOrientationIsValid(rawOrientation); if (!orientationIsValid) { if (this.history[jointIndex].FrameCount > 0) { rawOrientation = history[jointIndex].FilteredBoneOrientation; history[jointIndex].FrameCount = 0; } } // Initial start values or reset values if (this.history[jointIndex].FrameCount == 0) { // Use raw position and zero trend for first value filteredOrientation = rawOrientation; trend = Quaternion.identity; } else if (this.history[jointIndex].FrameCount == 1) { // Use average of two positions and calculate proper trend for end value Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation; filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f); Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.fCorrection); } else { // First apply a jitter filter Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation); float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter)); if (diffValJitter <= smoothingParameters.fJitterRadius) { filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.fJitterRadius); } else { filteredOrientation = rawOrientation; } // Now the double exponential smoothing filter filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, prevFilteredOrientation * prevTrend, smoothingParameters.fSmoothing); diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.fCorrection); } // Use the trend and predict into the future to reduce latency Quaternion predictedOrientation = filteredOrientation * KinectHelper.EnhancedQuaternionSlerp(Quaternion.identity, trend, smoothingParameters.fPrediction); // Check that we are not too far away from raw data Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation); float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff)); if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.fMaxDeviationRadius / diffVal); } // predictedOrientation.Normalize(); // filteredOrientation.Normalize(); // trend.Normalize(); // Save the data from this frame history[jointIndex].RawBoneOrientation = rawOrientation; history[jointIndex].FilteredBoneOrientation = filteredOrientation; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered and predicted data back into the bone orientation if (KinectHelper.BoneOrientationIsValid(predictedOrientation)) { jointOrientations[jointIndex].SetTRS(Vector3.zero, predictedOrientation, Vector3.one); } }
/// <summary> /// Update the filter for one joint. /// </summary> /// <param name="skeleton">The Skeleton to filter.</param> /// <param name="jt">The Skeleton Joint index to filter.</param> /// <param name="smoothingParameters">The Smoothing parameters to apply.</param> protected void FilterJoint(Skeleton skeleton, JointType jt, TransformSmoothParameters smoothingParameters) { if (null == skeleton) { return; } int jointIndex = (int)jt; Quaternion filteredOrientation; Quaternion trend; Quaternion rawOrientation = Quaternion.CreateFromRotationMatrix(KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[jt].HierarchicalRotation.Matrix)); Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation; Quaternion prevTrend = this.history[jointIndex].Trend; Vector3 rawPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[jt].Position); bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jt) && KinectHelper.BoneOrientationIsValid(rawOrientation); if (!orientationIsValid) { if (this.history[jointIndex].FrameCount > 0) { rawOrientation = this.history[jointIndex].FilteredBoneOrientation; this.history[jointIndex].FrameCount = 0; } } // Initial start values or reset values if (this.history[jointIndex].FrameCount == 0) { // Use raw position and zero trend for first value filteredOrientation = rawOrientation; trend = Quaternion.Identity; } else if (this.history[jointIndex].FrameCount == 1) { // Use average of two positions and calculate proper trend for end value Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation; filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f); Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.Correction); } else { // First apply a jitter filter Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation); float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter)); if (diffValJitter <= smoothingParameters.JitterRadius) { filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.JitterRadius); } else { filteredOrientation = rawOrientation; } // Now the double exponential smoothing filter filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, Quaternion.Multiply(prevFilteredOrientation, prevTrend), smoothingParameters.Smoothing); diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.Correction); } // Use the trend and predict into the future to reduce latency Quaternion predictedOrientation = Quaternion.Multiply(filteredOrientation, KinectHelper.EnhancedQuaternionSlerp(Quaternion.Identity, trend, smoothingParameters.Prediction)); // Check that we are not too far away from raw data Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation); float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff)); if (diffVal > smoothingParameters.MaxDeviationRadius) { predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.MaxDeviationRadius / diffVal); } predictedOrientation.Normalize(); filteredOrientation.Normalize(); trend.Normalize(); // Save the data from this frame this.history[jointIndex].RawBoneOrientation = rawOrientation; this.history[jointIndex].FilteredBoneOrientation = filteredOrientation; this.history[jointIndex].Trend = trend; this.history[jointIndex].FrameCount++; // Set the filtered and predicted data back into the bone orientation skeleton.BoneOrientations[jt].HierarchicalRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(predictedOrientation); // local rotation skeleton.BoneOrientations[jt].HierarchicalRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(predictedOrientation)); // HipCenter has no parent and is the root of our skeleton - leave the HipCenter absolute set as it is if (jt != JointType.HipCenter) { Quaternion parentRot = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[KinectHelper.ParentBoneJoint(jt)].AbsoluteRotation.Quaternion); // create a new world rotation Quaternion worldRot = Quaternion.Multiply(parentRot, predictedOrientation); skeleton.BoneOrientations[jt].AbsoluteRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(worldRot); skeleton.BoneOrientations[jt].AbsoluteRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(worldRot)); } else { // In the Hip Center root joint, absolute and relative are the same skeleton.BoneOrientations[jt].AbsoluteRotation.Quaternion = skeleton.BoneOrientations[jt].HierarchicalRotation.Quaternion; skeleton.BoneOrientations[jt].AbsoluteRotation.Matrix = skeleton.BoneOrientations[jt].HierarchicalRotation.Matrix; } }