Example #1
0
    public double CalculateDistanceByFormula(Windows.Kinect.Joint g1, Windows.Kinect.Joint g2)
    {
        Vector3 s = GetVector3FromJoint(g1);
        Vector3 t = GetVector3FromJoint(g2);

        return(Math.Sqrt(Math.Pow(s.x - t.x, 2) + Math.Pow(s.y - t.y, 2) + Math.Pow(s.z - t.z, 2)));
    }
Example #2
0
    void RefreshBodyObject(Windows.Kinect.Body body)
    {
        bool isGen = false;

        if (tt.Count - 1 != (int)Windows.Kinect.JointType.ThumbRight)
        {
            tt.Clear();
            isGen = true;
            print("ボーンを生成します.");
        }

        for (Windows.Kinect.JointType jt = Windows.Kinect.JointType.SpineBase; jt <= Windows.Kinect.JointType.ThumbRight; jt++)
        {
            Windows.Kinect.Joint sourceJoint = body.Joints[jt];
            Vector3 tmp = GetVector3FromJoint(sourceJoint) * 20;
            if (isGen)
            {
                var t = GameObject.CreatePrimitive(PrimitiveType.Cube).transform;
                t.name     = jt.ToString();
                t.position = tmp;
                t.parent   = transform;
                Destroy(t.GetComponent <BoxCollider>());
                tt.Add(t);
            }
            else
            {
                Transform t = tt[(int)jt];
                if (sourceJoint.TrackingState == Windows.Kinect.TrackingState.Tracked)
                {
                    t.position = tmp;
                    // t.gameObject.SetActive(true);
                }
                else
                {
                    // t.gameObject.SetActive(false);
                }
            }
        }
    }
Example #3
0
    private GameObject ConstructBone(Kinect.Joint source, Kinect.Joint dest)
    {
        // special cylinder instantiated between two points
        GameObject ob = GameObject.CreatePrimitive(PrimitiveType.Cylinder);

        if (ob.GetComponent <Collider>() != null)
        {
            Destroy(ob.GetComponent <Collider>());
        }

        // fake (but clever) 2d physics
        GameObject child = new GameObject("collider", typeof(CapsuleCollider));

        child.transform.SetParent(ob.transform);

        ob = UpdateBone(ob, source, dest);

        // always render on top
        //ob.layer = LayerMask.NameToLayer("KinectBody");
        ob.GetComponent <Renderer>().material = boneMaterial;

        return(ob);
    }
    private Vector3 GetVector3FromJoint(Kinect.Joint joint)
    {
        bool valid = joint.TrackingState != Kinect.TrackingState.NotTracked;

        if (convert_camera_ != null || valid)
        {
            Kinect.ColorSpacePoint point = coordinate_mapper_.MapCameraPointToColorSpace(joint.Position);
            Vector3 point2 = new Vector3(point.X, point.Y, 0);
            if ((point2.x >= 0) && (point2.x < kKinectWidth) && (point2.y >= 0) && (point2.y < kKinectHeight))
            {
                point2.x = point2.x * Screen.width / kKinectWidth;
                point2.y = point2.y * Screen.height / kKinectHeight;

                Vector3 color_point3 = convert_camera_.ScreenToWorldPoint(point2);

                color_point3.y *= -1;
                color_point3.z  = -1;
                return(color_point3);
            }
        }

        return(new Vector3(joint.Position.X * 10, joint.Position.Y * 10, joint.Position.Z * 10));
    }
Example #5
0
    private Ranges inBlurArea(Kinect.Joint joint)
    {
        float px = joint.Position.X;
        float pz = joint.Position.Z;

        if (px < BLUR_START_MIN_X)
        {
            return(Ranges.LEFT);
        }
        else if (px > BLUR_START_MAX_X)
        {
            return(Ranges.RIGHT);
        }
        else if (pz < BLUR_START_MIN_Z)
        {
            return(Ranges.FRONT);
        }
        else if (pz > BLUR_START_MAX_Z)
        {
            return(Ranges.BACK);
        }
        return(Ranges.NONE);
    }
Example #6
0
    /// <summary>
    /// Refreshes the current body updating
    /// the position of each of the limbs
    /// </summary>
    /// <param name="body">The Body to Update</param>
    /// <param name="bodyObject">The Body object holding all the pieces</param>
    private void RefreshHandPosition(Kinect.Body body, GameObject bodyObject)
    {
        if (!initialized)
        {
            handCursor       = Instantiate(handCursorObj, bodyObject.transform.position, Quaternion.identity);
            handCursorScript = handCursor.GetComponent <EnterNameHandCursor>();
            playerSpawnPosition.localEulerAngles = new Vector3(0, playerSpawnPosition.localEulerAngles.y - 90, playerSpawnPosition.localEulerAngles.z + 90);
            initialized = true;
        }

        Kinect.Joint rh = body.Joints[Kinect.JointType.HandRight];

        Transform currentJointObj = bodyObject.transform.Find(Kinect.JointType.HandRight.ToString());

        /// Find the Vector3 Position of this joint
        Vector3 currentJointPosition = BodySourceManager.GetVector3FromJoint(rh);

        currentJointObj.localPosition = new Vector3(currentJointPosition.x * -1, currentJointPosition.y, currentJointPosition.z);

        Vector3 handPos = (currentJointObj.position - perspectiveObj.position) * 0.5f + perspectiveObj.position;

        des = new Vector3(handPos.x, 2, handPos.z);
    }
    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];
            Kinect.Joint?targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.FindChild(jt.ToString());
            jointObj.localPosition = GetVector3FromJoint(sourceJoint);

            LineRenderer lr = jointObj.GetComponent <LineRenderer>();
            if (targetJoint.HasValue)
            {
                lr.SetPosition(0, jointObj.localPosition);
                lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
                lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
            }
            else
            {
                lr.enabled = false;
            }

            if (jt == Kinect.JointType.HandLeft)
            {
                LHand.transform.position = jointObj.transform.position;
            }
            if (jt == Kinect.JointType.HandRight)
            {
                RHand.transform.position = jointObj.transform.position;
            }
        }
    }
Example #8
0
    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];
            Kinect.Joint?targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.Find(jt.ToString());
            jointObj.localPosition = GetVector3FromJoint(sourceJoint);

            if (jt.ToString() == "SpineBase")
            {
                _boomStronk0.localPosition = GetVector3FromJoint(sourceJoint);
                _boomStronk0.localPosition = new Vector3(_boomStronk0.localPosition.x, _boomStronk0.localPosition.y, 0);
                //  jointObj.transform.localScale = new Vector3(5, 5, 5);
            }

            if (jt.ToString() == "SpineMid")
            {
                _boomStronk1.localPosition = GetVector3FromJoint(sourceJoint);
                _boomStronk1.localPosition = new Vector3(_boomStronk1.localPosition.x, _boomStronk1.localPosition.y, 0);
                //  jointObj.transform.localScale = new Vector3(5, 5, 5);
            }

            if (jt.ToString() == "Head")
            {
                _boomHoofd.localPosition = GetVector3FromJoint(sourceJoint);
                _boomHoofd.localPosition = new Vector3(_boomHoofd.localPosition.x, _boomHoofd.localPosition.y, 0);
                //  jointObj.transform.localScale = new Vector3(5, 5, 5);
            }
        }
    }
Example #9
0
/*    private void Align_Body()
 *  {
 *
 *  }*/

    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];
            Kinect.Joint?targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.Find(jt.ToString());
            jointObj.localPosition = GetVector3FromJoint(sourceJoint);

            LineRenderer lr = jointObj.GetComponent <LineRenderer>();
            if (targetJoint.HasValue)
            {
                lr.SetPosition(0, jointObj.position);
                lr.SetPosition(1, GetVector3FromScene(targetJoint.Value, bodyObject));
                lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
            }
            else
            {
                lr.enabled = false;
            }

            /* VIVE-KINECT CALIBRATION
             * if (jt == Kinect.JointType.Head)
             * {
             *  jointObj.tag = "Head";
             *  Vector3 headOffset = GetVector3FromJoint(sourceJoint); //Offset of head node from body
             *  bodyObject.transform.position = new Vector3(mainCamera.transform.position.x - headOffset.x, mainCamera.transform.position.y - headOffset.y, mainCamera.transform.position.z - headOffset.z); //Sets body pos
             * }
             * END */
        }
    }
Example #10
0
        private static Vector3 ScaleXY(Vector3 orig, Windows.Kinect.Joint joint, Vector3 jointSpinePos)
        {
            float opp1X = joint.Position.X - jointSpinePos.x;
            float adj2  = orig.z;
            float adj1  = joint.Position.Z;

            float opp1Y = joint.Position.Y - jointSpinePos.y;

            float scaleX = opp1X * adj2 / adj1;
            float scaleY = opp1Y * adj2 / adj1;

            //if (joint.JointType.Equals(JointType.WristRight))
            //{
            //    Debug.Log("Scaling for joint " + joint.JointType.ToString() + "...");
            //    Debug.Log("\tXScale = " + scaleX);
            //    Debug.Log("\tYScale = " + scaleY);
            //    Debug.Log("\tOrig X = " + orig.x);
            //    Debug.Log("\tOrig Y = " + orig.y);
            //    Debug.Log("\tjoint Pos.z = " + adj1);
            //    Debug.Log("\tadj2 = " + adj2);
            //}

            return(orig + new Vector3(Mathf.Abs(orig.x * scaleX), Mathf.Abs(orig.y * scaleY), 0));
        }
Example #11
0
    public void RefreshBodyObject()
    {
        foreach (JointType jt in Kinect.EssentialJoints)
        {
            Windows.Kinect.Joint sourceJoint = _trackedBody.Joints[jt];

            if (Kinect.BoneMap.ContainsKey(jt))
            {
            }

            Transform jointObj = BodyTransforms[jt];
            Vector3   pos      = GetVector3FromJoint(sourceJoint);
            BodyFilters[jt].record(pos);
        }
        foreach (JointType jt in Kinect.UnessentialJoints)
        {
            Windows.Kinect.Joint sourceJoint = _trackedBody.Joints[jt];
            Transform            jointObj    = BodyTransforms[jt];
            Vector3 pos = GetVector3FromJoint(sourceJoint);
            BodyPositions[jt] = GetVector3FromJoint(sourceJoint);
        }
        _rightHandFilter.Record(_trackedBody.HandRightState == HandState.Closed);
        _leftHandFilter.Record(_trackedBody.HandLeftState == HandState.Closed);
    }
Example #12
0
    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];
            Kinect.Joint?targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.Find(jt.ToString());
            jointObj.localPosition = GetVector3FromJoint(sourceJoint);

            LineRenderer lr = jointObj.GetComponent <LineRenderer>();
            if (targetJoint.HasValue)
            {
                lr.SetPosition(0, jointObj.localPosition);
                lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
                lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
            }
            else
            {
                lr.enabled = false;
            }
        }

        //float shoulderElbowAngleL = GetAngle(body, Kinect.JointType.ShoulderLeft, Kinect.JointType.ElbowLeft);
        //Debug.Log(shoulderElbowAngleL);
        //float hipKneeAngleL = GetAngle(body, Kinect.JointType.HipLeft, Kinect.JointType.KneeLeft);
        //Debug.Log(hipKneeAngleL);
        float hipKneeAngleR = GetAngle(body, Kinect.JointType.HipRight, Kinect.JointType.KneeRight);

        Debug.Log(hipKneeAngleR);
    }
Example #13
0
    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        JointTransforms.Clear();
        JointOrientations.Clear();
        bodyTracked = body.IsTracked;
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];
            Kinect.Joint?targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.FindChild(jt.ToString());

            // UPDATE STATIC VAR
            JointTransforms.Add(jointObj);
            JointOrientations.Add(body.JointOrientations[jt]);

            jointObj.localPosition = GetVector3FromJoint(sourceJoint);

            LineRenderer lr = jointObj.GetComponent <LineRenderer>();
            if (targetJoint.HasValue)
            {
                lr.SetPosition(0, jointObj.localPosition);
                lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
                lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
            }
            else
            {
                lr.enabled = false;
            }
        }
    }
Example #14
0
 // return current position of given joint
 private Vector3 GetVector3FromJoint(Kinect.Joint joint)
 {
     // return position with scale to enlarge skeleton
     return(new Vector3(joint.Position.X * SkeletonScale, joint.Position.Y * SkeletonScale, joint.Position.Z * SkeletonScale));
 }
Example #15
0
    public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld)
    {
        bool bNewFrame = false;

        if ((multiSourceFrameReader != null && multiSourceFrame != null) ||
            bodyFrameReader != null)
        {
            var frame = multiSourceFrame != null?multiSourceFrame.BodyFrameReference.AcquireFrame() :
                            bodyFrameReader.AcquireLatestFrame();

            if (frame != null)
            {
                frame.GetAndRefreshBodyData(bodyData);
                bodyFrame.liRelativeTime = frame.RelativeTime.Ticks;

                frame.Dispose();
                frame = null;

                for (int i = 0; i < sensorData.bodyCount; i++)
                {
                    Body body = bodyData[i];

                    if (body == null)
                    {
                        bodyFrame.bodyData[i].bIsTracked = 0;
                        continue;
                    }

                    bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0);

                    if (body.IsTracked)
                    {
                        // transfer body and joints data
                        //传送身体和关节数据
                        bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId;

                        for (int j = 0; j < sensorData.jointCount; j++)
                        {
                            Windows.Kinect.Joint    joint     = body.Joints[(Windows.Kinect.JointType)j];
                            KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j];

                            jointData.jointType     = (KinectInterop.JointType)j;
                            jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState;

                            if ((int)joint.TrackingState != (int)TrackingState.NotTracked)
                            {
                                jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
                                jointData.position  = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos);
                            }

                            jointData.orientation = Quaternion.identity;
//							Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation;
//							jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W);

                            if (j == 0)
                            {
                                bodyFrame.bodyData[i].position    = jointData.position;
                                bodyFrame.bodyData[i].orientation = jointData.orientation;
                            }

                            bodyFrame.bodyData[i].joint[j] = jointData;
                        }

                        // tranfer hand states
                        //转变手的状态
                        bodyFrame.bodyData[i].leftHandState      = (KinectInterop.HandState)body.HandLeftState;
                        bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence;

                        bodyFrame.bodyData[i].rightHandState      = (KinectInterop.HandState)body.HandRightState;
                        bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence;
                    }
                }

                bNewFrame = true;
            }
        }

        return(bNewFrame);
    }
Example #16
0
    public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld)
    {
        bool bNewFrame = false;

        if ((multiSourceFrameReader != null && multiSourceFrame != null) ||
            bodyFrameReader != null)
        {
            var frame = multiSourceFrame != null?multiSourceFrame.BodyFrameReference.AcquireFrame() :
                            bodyFrameReader.AcquireLatestFrame();

            if (frame != null)
            {
                frame.GetAndRefreshBodyData(bodyData);
                bodyFrame.liRelativeTime = frame.RelativeTime.Ticks;

                if (sensorData.hintHeightAngle)
                {
                    // get the floor plane
                    Windows.Kinect.Vector4 vFloorPlane = frame.FloorClipPlane;
                    Vector3 floorPlane = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z);

                    sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up);
                    sensorData.sensorHgtDetected = vFloorPlane.W;
                }

                frame.Dispose();
                frame = null;

                for (int i = 0; i < sensorData.bodyCount; i++)
                {
                    Body body = bodyData[i];

                    if (body == null)
                    {
                        bodyFrame.bodyData[i].bIsTracked = 0;
                        continue;
                    }

                    bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0);

                    if (body.IsTracked)
                    {
                        // transfer body and joints data
                        bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId;

                        // cache the body joints (following the advice of Brian Chasalow)
                        Dictionary <Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints;

                        for (int j = 0; j < sensorData.jointCount; j++)
                        {
                            Windows.Kinect.Joint    joint     = bodyJoints[(Windows.Kinect.JointType)j];
                            KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j];

                            jointData.jointType     = (KinectInterop.JointType)j;
                            jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState;

                            if ((int)joint.TrackingState != (int)TrackingState.NotTracked)
                            {
                                jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
                                jointData.position  = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos);
                            }

                            jointData.orientation = Quaternion.identity;
//							Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation;
//							jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W);

                            if (j == 0)
                            {
                                bodyFrame.bodyData[i].position    = jointData.position;
                                bodyFrame.bodyData[i].orientation = jointData.orientation;
                            }

                            bodyFrame.bodyData[i].joint[j] = jointData;
                        }

                        // tranfer hand states
                        bodyFrame.bodyData[i].leftHandState      = (KinectInterop.HandState)body.HandLeftState;
                        bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence;

                        bodyFrame.bodyData[i].rightHandState      = (KinectInterop.HandState)body.HandRightState;
                        bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence;
                    }
                }

                bNewFrame = true;
            }
        }

        return(bNewFrame);
    }
Example #17
0
    void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
    {
        CameraSpacePoint vPrevRawPosition;
        CameraSpacePoint vPrevFilteredPosition;
        CameraSpacePoint vPrevTrend;
        CameraSpacePoint vRawPosition;
        CameraSpacePoint vFilteredPosition;
        CameraSpacePoint vPredictedPosition;
        CameraSpacePoint vDiff;
        CameraSpacePoint vTrend;
        float            fDiff;
        bool             bJointIsValid;

        Windows.Kinect.Joint joint = body.Joints[jt];

        vRawPosition          = joint.Position;
        vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
        vPrevTrend            = m_pHistory[(int)jt].m_vTrend;
        vPrevRawPosition      = m_pHistory[(int)jt].m_vRawPosition;
        bJointIsValid         = JointPositionIsValid(vRawPosition);

        // If joint is invalid, reset the filter
        if (!bJointIsValid)
        {
            m_pHistory[(int)jt].m_dwFrameCount = 0;
        }

        // Initial start values
        if (m_pHistory[(int)jt].m_dwFrameCount == 0)
        {
            vFilteredPosition = vRawPosition;
            vTrend            = CSVectorZero();
            m_pHistory[(int)jt].m_dwFrameCount++;
        }
        else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
        {
            vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
            vDiff             = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
            vTrend            = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
            m_pHistory[(int)jt].m_dwFrameCount++;
        }
        else
        {
            // First apply jitter filter
            vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
            fDiff = CSVectorLength(vDiff);

            if (fDiff <= smoothingParams.fJitterRadius)
            {
                vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
                                                CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
            }
            else
            {
                vFilteredPosition = vRawPosition;
            }

            // Now the double exponential smoothing filter
            vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
                                            CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));


            vDiff  = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
            vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
        }

        // Predict into the future to reduce latency
        vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));

        // Check that we are not too far away from raw data
        vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
        fDiff = CSVectorLength(vDiff);

        if (fDiff > smoothingParams.fMaxDeviationRadius)
        {
            vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
                                             CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
        }

        // Save the data from this frame
        m_pHistory[(int)jt].m_vRawPosition      = vRawPosition;
        m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
        m_pHistory[(int)jt].m_vTrend            = vTrend;

        // Output the data
        m_pFilteredJoints[(int)jt] = vPredictedPosition;
    }
Example #18
0
    // Polls for new skeleton data
    public static bool PollBodyData(SensorData sensorData, ref BodyFrameData bodyFrame)
    {
        bool bNewFrame = false;

//		BodyFrameData tempBodyFrame = new BodyFrameData();
//		int hr = GetBodyFrameData(ref tempBodyFrame, true, true);
//
////		if((hr == 0) && ((sensorData.colorImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastColorFrameTime)) &&
////		   ((sensorData.depthImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastDepthFrameTime)))
//		{
//			if(hr == 0 /**&& (bodyFrame.liRelativeTime > lastFrameTime)*/)
//			{
//				bodyFrame = tempBodyFrame;
//				sensorData.lastBodyFrameTime = bodyFrame.liRelativeTime;
//				bNewFrame = true;
//			}
//		}

        if (sensorData.bodyFrameReader != null)
        {
            var frame = sensorData.bodyFrameReader.AcquireLatestFrame();

            if (frame != null)
            {
                frame.GetAndRefreshBodyData(sensorData.bodyData);
                bodyFrame.liRelativeTime = frame.RelativeTime;

                frame.Dispose();
                frame = null;

                for (int i = 0; i < sensorData.bodyCount; i++)
                {
                    Body body = sensorData.bodyData[i];

                    if (body == null)
                    {
                        bodyFrame.bodyData[i].bIsTracked = 0;
                        continue;
                    }

                    bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0);
                    if (body.IsTracked)
                    {
                        bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId;

                        for (int j = 0; j < Constants.JointCount; j++)
                        {
                            Windows.Kinect.Joint joint     = body.Joints[(JointType)j];
                            JointData            jointData = bodyFrame.bodyData[i].joint[j];

                            jointData.jointType     = (JointType)j;
                            jointData.trackingState = joint.TrackingState;

                            jointData.position  = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
                            jointData.kinectPos = jointData.position;

                            //Debug.Log(jointData.jointType + " - " + joint.TrackingState + " " + jointData.kinectPos);

                            if (j == 0)
                            {
                                jointData.direction = Vector3.zero;
                            }
                            else
                            {
                                int jParent = (int)GetParentJoint(jointData.jointType);

                                if (jointData.trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked)
                                {
                                    jointData.direction = jointData.kinectPos - bodyFrame.bodyData[i].joint[jParent].kinectPos;
                                }
                            }

                            jointData.orientation = Quaternion.identity;
//							Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation;
//							Quaternion orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W);
//
//							if(j != 0)
//							{
//								Quaternion parentOri = bodyFrame.bodyData[i].joint[jParent].orientation;
//								jointData.orientation = parentOri * orientation;
//							}
//							else
//							{
//								jointData.orientation = orientation;
//							}

                            if (j == 0)
                            {
                                bodyFrame.bodyData[i].position    = jointData.position;
                                bodyFrame.bodyData[i].orientation = jointData.orientation;
                            }

                            bodyFrame.bodyData[i].joint[j] = jointData;
                        }

                        bodyFrame.bodyData[i].leftHandState      = body.HandLeftState;
                        bodyFrame.bodyData[i].leftHandConfidence = body.HandLeftConfidence;

                        bodyFrame.bodyData[i].rightHandState      = body.HandRightState;
                        bodyFrame.bodyData[i].rightHandConfidence = body.HandRightConfidence;
                    }
                }

                bNewFrame = true;
            }
        }

        return(bNewFrame);
    }
 public static Vector3 GetJointPosition2D(Windows.Kinect.Joint joint, float z = 0f)
 {
     // TODO: do projection space calculations here instead of 10x values
     return(new Vector3(joint.Position.X, joint.Position.Y * 10, z));
 }
Example #20
0
        private Vector3 GetBoneVector(Joint parentJoint, Joint childJoint)
        {
            var parentVector = parentJoint.Position;
            var childVector = childJoint.Position;

            var vector = new Vector3(childVector.X - parentVector.X,
                                     childVector.Y - parentVector.Y,
                                     childVector.Z - parentVector.Z);

            return vector;
        }
Example #21
0
 private static Vector3 GetVector3FromJoint(Kinect2.Joint joint)
 {
     return(new Vector3(joint.Position.X, joint.Position.Y + 1.0f, -joint.Position.Z + 2.0f));
 }
Example #22
0
 private static Vector3 GetVector3FromJoint(Windows.Kinect.Joint joint)
 {
     return(new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z));
 }
Example #23
0
 public float CalculateDistance(Windows.Kinect.Joint g1, Windows.Kinect.Joint g2)
 {
     return(Vector3.Distance(GetVector3FromJoint(g1), GetVector3FromJoint(g2)));
 }
Example #24
0
 public bool Equals(Joint obj)
 {
     return JointType.Equals(obj.JointType) && Position.Equals(obj.Position) && TrackingState.Equals(obj.TrackingState);
 }
Example #25
0
    private void PlayerMovement(Kinect.Joint PlayerPosition)
    {
        float MoveBuffer = 2.25f;

        transform.position = new Vector3(-PlayerPosition.Position.X + InitialPosition.x, InitialPosition.y, (PlayerPosition.Position.Z - MoveBuffer + InitialPosition.z));
    }
 //Joint should be of JointType "joinTypeToAttachTo"
 public abstract void UpdatePosition(ulong uniqueId, Kinect.Joint joint, Vector3 jointWorldPosition);
Example #27
0
    private void UpdateBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        if (!sitTest)
        {
            for (Kinect.JointType jt = Kinect.JointType.HipLeft; jt <= Kinect.JointType.ThumbRight; jt++)
            {
                Kinect.Joint sourceJoint = body.Joints[jt];
                Kinect.Joint?targetJoint = null;

                //IF JOINT IS IN BONEMAP ADD IT TO TARGET JOINT
                if (_BoneMap.ContainsKey(jt))
                {
                    targetJoint = body.Joints[_BoneMap[jt]];
                }

                Transform jointObj = bodyObject.transform.Find(jt.ToString());
                jointObj.localPosition = GetVector3FromJoint(sourceJoint);

                LineRenderer lr = jointObj.GetComponent <LineRenderer>();
                if (targetJoint.HasValue)
                {
                    lr.SetPosition(0, jointObj.localPosition);
                    lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
                    lr.startColor = Color.white; //(Color.grey,Color.grey);
                    lr.endColor   = Color.grey;

                    float legAngle = FindHipAngle(GetVector3FromJoint(sourceJoint), GetVector3FromJoint(targetJoint.Value));
                    //Debug.Log(legAngle);

                    //-5 <= x <= 5 -> tollerence TOTEST
                    if (legAngle >= -5 && legAngle <= 5)
                    {
                        sitTest = true;


                        //ghetto solution -> couldnt get the face to be not visible at start --> set face after sit
                    }
                }
                else
                {
                    lr.enabled = false;
                }
            }
        }

        if (sitTest)
        {
            if (!skelTest)
            {
                //iterate over all the joints that arent hands and remover them
                for (Kinect.JointType jt = Kinect.JointType.HipLeft; jt <= Kinect.JointType.ThumbRight; jt++)
                {
                    if (jt != Kinect.JointType.HandRight || jt != Kinect.JointType.HandLeft)
                    {
                        GameObject.Find(jt.ToString()).SetActive(false);
                    }
                }
                skelTest = true;
            }


            //for face and hands (single joints)
            foreach (Kinect.JointType _joint in _joints)
            {
                //get new target position
                Joint sourceJoint = body.Joints[_joint];

                Vector3 targetPosition = GetVector3FromJoint(sourceJoint);
                //all play happens on same plane
                targetPosition.z = 0;

                //get joint, set new postion
                Transform jointObject = bodyObject.transform.Find(_joint.ToString());
                jointObject.position = targetPosition;

                Joint selectedJoint = body.Joints[_joint];

                if (_joint.ToString() != "Head")
                {
                    if (_joint.ToString() == "HandLeft")
                    {
                        leftHandPos = GetVector3FromJoint(selectedJoint);
                    }
                    else
                    {
                        rightHandPos = GetVector3FromJoint(selectedJoint);
                    }
                }

                //0.5 is radius
                if (leftHandPos.x <= -4.5 && leftHandPos.x >= -5.5)
                {
                    //Debug.Log(rightHandPos.y);
                }
            }
        }
    }
Example #28
0
        private BoneTrackingState GetBoneTrackingState(Joint nearJoint, Joint farJoint)
        {
            var nearState = nearJoint.TrackingState;
            var farState = farJoint.TrackingState;

            //T + T => T
            //T + I => I
            //I + T => I
            //I + I => I

            if (nearState == TrackingState.Tracked && farState == TrackingState.Tracked)
            {
                return BoneTrackingState.Tracked;
            }

            if (nearState == TrackingState.NotTracked || farState == TrackingState.NotTracked)
            {
                return BoneTrackingState.NotTracked;
            }

            //Can't be both tracked, and neither are not tracked
            //Must be inferred and either tracking or inferred
            return BoneTrackingState.Inferred;
        }
Example #29
0
 public Vector3 ToVector3(Windows.Kinect.Joint j) =>
 new Vector3(j.Position.X, j.Position.Y, j.Position.Z);
Example #30
0
    // Update is called once per frame
    void Update()
    {
        if (_BodyManager == null)
        {
            Debug.Log("_BodyManager == null");
            return;
        }

        // Bodyデータを取得する
        var data = _BodyManager.GetData();

        if (data == null)
        {
            return;
        }

        // 最初に追跡している人を取得する
        var body = data.FirstOrDefault(b => b.IsTracked);

        if (body == null)
        {
            return;
        }

        Kinect.Joint sourceJoint = body.Joints[Kinect.JointType.SpineBase];
        Debug.Log("Z:" + sourceJoint.Position.Z);
        if (sourceJoint.Position.Z <= 0.2 || sourceJoint.Position.Z >= 1.5)
        {
            return;
        }


        // 床の傾きを取得する
        var floorPlane = _BodyManager.FloorClipPlane;
        var comp       = Quaternion.FromToRotation(
            new Vector3(floorPlane.X, floorPlane.Y, floorPlane.Z), Vector3.up);

        // 関節の回転を取得する
        var joints = body.JointOrientations;

        Quaternion SpineBase;
        Quaternion SpineMid;
        Quaternion SpineShoulder;
        Quaternion ShoulderLeft;
        Quaternion ShoulderRight;
        Quaternion ElbowLeft;
        Quaternion WristLeft;
        Quaternion HandLeft;
        Quaternion ElbowRight;
        Quaternion WristRight;
        Quaternion HandRight;
        Quaternion KneeLeft;
        Quaternion AnkleLeft;
        Quaternion KneeRight;
        Quaternion AnkleRight;

        // 鏡
        if (IsMirror)
        {
            SpineBase     = joints[JointType.SpineBase].Orientation.ToMirror().ToQuaternion(comp);
            SpineMid      = joints[JointType.SpineMid].Orientation.ToMirror().ToQuaternion(comp);
            SpineShoulder = joints[JointType.SpineShoulder].Orientation.ToMirror().ToQuaternion(comp);
            ShoulderLeft  = joints[JointType.ShoulderRight].Orientation.ToMirror().ToQuaternion(comp);
            ShoulderRight = joints[JointType.ShoulderLeft].Orientation.ToMirror().ToQuaternion(comp);
            ElbowLeft     = joints[JointType.ElbowRight].Orientation.ToMirror().ToQuaternion(comp);
            WristLeft     = joints[JointType.WristRight].Orientation.ToMirror().ToQuaternion(comp);
            HandLeft      = joints[JointType.HandRight].Orientation.ToMirror().ToQuaternion(comp);
            ElbowRight    = joints[JointType.ElbowLeft].Orientation.ToMirror().ToQuaternion(comp);
            WristRight    = joints[JointType.WristLeft].Orientation.ToMirror().ToQuaternion(comp);
            HandRight     = joints[JointType.HandLeft].Orientation.ToMirror().ToQuaternion(comp);
            KneeLeft      = joints[JointType.KneeRight].Orientation.ToMirror().ToQuaternion(comp);
            AnkleLeft     = joints[JointType.AnkleRight].Orientation.ToMirror().ToQuaternion(comp);
            KneeRight     = joints[JointType.KneeLeft].Orientation.ToMirror().ToQuaternion(comp);
            AnkleRight    = joints[JointType.AnkleLeft].Orientation.ToMirror().ToQuaternion(comp);
        }
        // そのまま
        else
        {
            SpineBase     = joints[JointType.SpineBase].Orientation.ToQuaternion(comp);
            SpineMid      = joints[JointType.SpineMid].Orientation.ToQuaternion(comp);
            SpineShoulder = joints[JointType.SpineShoulder].Orientation.ToQuaternion(comp);
            ShoulderLeft  = joints[JointType.ShoulderLeft].Orientation.ToQuaternion(comp);
            ShoulderRight = joints[JointType.ShoulderRight].Orientation.ToQuaternion(comp);
            ElbowLeft     = joints[JointType.ElbowLeft].Orientation.ToQuaternion(comp);
            WristLeft     = joints[JointType.WristLeft].Orientation.ToQuaternion(comp);
            HandLeft      = joints[JointType.HandLeft].Orientation.ToQuaternion(comp);
            ElbowRight    = joints[JointType.ElbowRight].Orientation.ToQuaternion(comp);
            WristRight    = joints[JointType.WristRight].Orientation.ToQuaternion(comp);
            HandRight     = joints[JointType.HandRight].Orientation.ToQuaternion(comp);
            KneeLeft      = joints[JointType.KneeLeft].Orientation.ToQuaternion(comp);
            AnkleLeft     = joints[JointType.AnkleLeft].Orientation.ToQuaternion(comp);
            KneeRight     = joints[JointType.KneeRight].Orientation.ToQuaternion(comp);
            AnkleRight    = joints[JointType.AnkleRight].Orientation.ToQuaternion(comp);
        }

        // 関節の回転を計算する
        var q = Ref.transform.rotation;

        Ref.transform.rotation = Quaternion.identity;

        var comp2 = Quaternion.AngleAxis(90, new Vector3(0, 1, 0)) *
                    Quaternion.AngleAxis(-90, new Vector3(0, 0, 1));

        Spine1.transform.rotation = SpineMid * comp2;

        RightArm.transform.rotation     = ElbowRight * comp2;
        RightForeArm.transform.rotation = WristRight * comp2;
        RightHand.transform.rotation    = HandRight * comp2;

        LeftArm.transform.rotation     = ElbowLeft * comp2;
        LeftForeArm.transform.rotation = WristLeft * comp2;
        LeftHand.transform.rotation    = HandLeft * comp2;

        RightUpLeg.transform.rotation = KneeRight * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1));
        RightLeg.transform.rotation   = AnkleRight * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1));

        LeftUpLeg.transform.rotation = KneeLeft * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1));
        LeftLeg.transform.rotation   = AnkleLeft * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1));

        // モデルの回転を設定する
        Ref.transform.rotation = q;

        // モデルの位置を移動する
        var pos = body.Joints[JointType.SpineMid].Position;

        Ref.transform.position = new Vector3(-pos.X, pos.Y, -pos.Z);
    }
    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        int i = 0;

        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.HandRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt]; //size ?

            Kinect.Joint?targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.FindChild(jt.ToString());
            if (jt == Kinect.JointType.HandRight)
            {
                jointObj.localPosition = GetVector3FromJoint(sourceJoint) + bodyOffset + rightHandOffset;
            }
            else
            {
                jointObj.localPosition = GetVector3FromJoint(sourceJoint) + bodyOffset;
            }
            Vector3 transPosition = GetVector3FromJoint(sourceJoint) + bodyOffset;  //’�ˆÓ

            Debug.Log(transPosition.z);
            Debug.Log(jointObj.localPosition.z);


            positionFloats[(i * 3)]     = transPosition.x;
            positionFloats[(i * 3) + 1] = transPosition.y;
            positionFloats[(i * 3) + 2] = transPosition.z;

            i++;

            LineRenderer lr = jointObj.GetComponent <LineRenderer>();
            if (targetJoint.HasValue)
            {
                lr.SetPosition(0, jointObj.localPosition);
                if (jt == Kinect.JointType.HandRight)
                {
                    lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value) + bodyOffset + rightHandOffset);
                }
                else
                {
                    lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value) + bodyOffset);
                }
            }
            else
            {
                lr.enabled = false;
            }
        }

        /*for (Kinect.JointType jt = Kinect.JointType.HandRight+1; jt <= Kinect.JointType.ThumbRight; jt++) {
         *  Kinect.Joint sourceJoint = body.Joints[jt]; //size ?
         *  Vector3 transPosition = GetVector3FromJointforSend(sourceJoint);
         *
         *  positionFloats[(i * 3)] = transPosition.x;
         *  positionFloats[(i * 3) + 1] = transPosition.y;
         *  positionFloats[(i * 3) + 2] = transPosition.z;
         *
         *  i++;
         * }*/

        if (Input.GetMouseButton(2))
        {
            i = 0;
            if (Input.GetMouseButtonDown(2))
            {
                streamWriter.Close();
                boneNumber++;
                streamWriter = new StreamWriter("./bone" + boneNumber + ".txt", true);
            }

            for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.HandRight; jt++)
            {
                //Start Record
                i++;
            }
            streamWriter.Write(":");
            streamWriter.Flush();
        }
    }
 private Vector3 GetVector3FromJoint(Kinect.Joint joint)
 {
     //Debug.Log("joint = " + joint.Position.X + " y: "+ joint.Position.Y + " z: " + joint.Position.Z);
     return(new Vector3(joint.Position.X * boneSize * xTransferAmount + xOffset, joint.Position.Y * boneSize * yTransferAmount + yOffset, joint.Position.Z * boneSize * -1 * zTransferAmount + zOffset));
 }
Example #33
0
 private static Vector3 GetVector3FromJoint(Kinect.Joint joint)
 {
     return(new Vector3(joint.Position.X * 10, joint.Position.Y * 10, joint.Position.Z * 10));
 }
    /// <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(ref Kinect.Body skeleton, Kinect.JointType jt, TransformSmoothParameters smoothingParameters)
    {
        if (null == skeleton)
            {
                return;
            }

        //	Debug.Log("here");
            int jointIndex = (int)jt;

            Vector3 filteredPosition;
            Vector3 diffvec;
            Vector3 trend;
            float diffVal;

            Vector3 rawPosition = GetVector3FromJoint(skeleton.Joints[jt]);
            Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition;
            Vector3 prevTrend = this.history[jointIndex].Trend;
            Vector3 prevRawPosition = this.history[jointIndex].RawPosition;
            bool jointIsValid = JointPositionIsValid(rawPosition);
        /*if(jt == Kinect.JointType.HandLeft)
            Debug.Log ("raw "  +":"+ rawPosition.x + ", " + rawPosition.y + ", " + rawPosition.z);
        */
            // If joint is invalid, reset the filter
            if (!jointIsValid)
            {
                this.history[jointIndex].FrameCount = 0;
            }

            // Initial start values
            if (this.history[jointIndex].FrameCount == 0)
            {
                filteredPosition = rawPosition;
                trend = Vector3.zero;
        //			if(jt == Kinect.JointType.HandLeft)
        //				Debug.Log ("filtered pos" +":"+ filteredPosition.x + ", " + filteredPosition.z + ", " + filteredPosition.z);

            }
            else if (this.history[jointIndex].FrameCount == 1)
            {
                filteredPosition = (rawPosition+ prevRawPosition) * 0.5f;
                diffvec = filteredPosition - prevFilteredPosition;
                trend = (diffvec * smoothingParameters.Correction) + (prevTrend * (1.0f - smoothingParameters.Correction));
            }
            else
            {
                // First apply jitter filter
                diffvec = rawPosition - prevFilteredPosition;
                diffVal = Mathf.Abs(diffvec.magnitude);

                if (diffVal <= smoothingParameters.JitterRadius)
                {
                    filteredPosition = (rawPosition * (diffVal / smoothingParameters.JitterRadius)) + (prevFilteredPosition * ( 1.0f - (diffVal / smoothingParameters.JitterRadius)));
                }
                else
                {
                    filteredPosition = rawPosition;
                }

                // Now the double exponential smoothing filter
                filteredPosition = (filteredPosition *(1.0f - smoothingParameters.Smoothing)) +  ((prevFilteredPosition + prevTrend) * smoothingParameters.Smoothing);
        //	if(jt == Kinect.JointType.HandLeft)
            //					Debug.Log ("filtered pos" +":"+ filteredPosition.x + ", " + filteredPosition.z + ", " + filteredPosition.z);
                diffvec = filteredPosition - prevFilteredPosition;
                trend = (diffvec * smoothingParameters.Correction) + (prevTrend *( 1.0f - smoothingParameters.Correction));

            }

            // Predict into the future to reduce latency
            Vector3 predictedPosition = filteredPosition + (trend *smoothingParameters.Prediction);

            // Check that we are not too far away from raw data
            diffvec = predictedPosition- rawPosition;
            diffVal = Mathf.Abs(diffvec.magnitude);

            if (diffVal > smoothingParameters.MaxDeviationRadius)
            {
                predictedPosition = (predictedPosition * (smoothingParameters.MaxDeviationRadius / diffVal)) + (rawPosition* ( 1.0f - (smoothingParameters.MaxDeviationRadius / diffVal)));
            }

            // Save the data from this frame
            this.history[jointIndex].RawPosition = rawPosition;
            this.history[jointIndex].FilteredPosition = filteredPosition;
            this.history[jointIndex].Trend = trend;
            this.history[jointIndex].FrameCount++;
        //Debug.Log (this.history[jointIndex].FrameCount);
            // Set the filtered data back into the joint
            Kinect.Joint j = new Kinect.Joint();
        j.Position = Vector3ToSkeletonPoint(predictedPosition);
        //if(skeleton.Joints.ContainsKey(jt))
            //skeleton.Joints.Remove(jt);
         	FilteredJoints[jt] = j;// =Vector3ToSkeletonPoint(predictedPosition);// j;

        /*	if (jt == Kinect.JointType.HandLeft) {
                        Debug.Log ("predicted " + ":" + j.Position.X + ", " + j.Position.Y + ", " + j.Position.Z);
                        //Debug.Log ("new " + ":" + skeleton.Joints[jt].Position.X + ", " + skeleton.Joints[jt].Position.Y + ", " + skeleton.Joints[jt].Position.Z);
                }*/
    }