locked
Joint Smoothing code RRS feed

  • General discussion

  • Here is some code to implement a method of joint smoothing for c++. There are dependencies on having certain functions available in the framework you choose.

    //--------------------------------------------------------------------------------------
    // KinectJointFilter.h
    //
    // This file contains Holt Double Exponential Smoothing filter for filtering Joints
    //
    // Copyright (C) Microsoft Corporation. All rights reserved.
    //--------------------------------------------------------------------------------------
    
    #pragma once
    
    #include <DirectXMath.h>
    #include <queue>
    
    namespace Sample
    {
        typedef struct _TRANSFORM_SMOOTH_PARAMETERS
        {
            FLOAT   fSmoothing;             // [0..1], lower values closer to raw data
            FLOAT   fCorrection;            // [0..1], lower values slower to correct towards the raw data
            FLOAT   fPrediction;            // [0..n], the number of frames to predict into the future
            FLOAT   fJitterRadius;          // The radius in meters for jitter reduction
            FLOAT   fMaxDeviationRadius;    // The maximum radius in meters that filtered positions are allowed to deviate from raw data
        } TRANSFORM_SMOOTH_PARAMETERS;
    
        // Holt Double Exponential Smoothing filter
        class FilterDoubleExponentialData
        {
        public:
            DirectX::XMVECTOR m_vRawPosition;
            DirectX::XMVECTOR m_vFilteredPosition;
            DirectX::XMVECTOR m_vTrend;
            DWORD    m_dwFrameCount;
        };
    
        class FilterDoubleExponential
        {
        public:
            FilterDoubleExponential() { Init(); }
            ~FilterDoubleExponential() { Shutdown(); }
    
            VOID Init(FLOAT fSmoothing = 0.25f, FLOAT fCorrection = 0.25f, FLOAT fPrediction = 0.25f, FLOAT fJitterRadius = 0.03f, FLOAT fMaxDeviationRadius = 0.05f)
            {
                Reset(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
            }
    
            VOID Shutdown()
            {
            }
    
            VOID Reset(FLOAT fSmoothing = 0.25f, FLOAT fCorrection = 0.25f, FLOAT fPrediction = 0.25f, FLOAT fJitterRadius = 0.03f, FLOAT fMaxDeviationRadius = 0.05f)
            {
                assert(m_pFilteredJoints);
                assert(m_pHistory);
    
                m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
                m_fSmoothing = fSmoothing;                   // How much smothing will occur.  Will lag when too high
                m_fCorrection = fCorrection;                 // How much to correct back from prediction.  Can make things springy
                m_fPrediction = fPrediction;                 // Amount of prediction into the future to use. Can over shoot when too high
                m_fJitterRadius = fJitterRadius;             // Size of the radius where jitter is removed. Can do too much smoothing when too high
    
                memset(m_pFilteredJoints, 0, sizeof(DirectX::XMVECTOR) * JointType_Count);
                memset(m_pHistory, 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
            }
    
            void Update(IBody* const pBody);
            void Update(Joint joints[]);
    
            inline const DirectX::XMVECTOR* GetFilteredJoints() const { return &m_pFilteredJoints[0]; }
    
        private:
            DirectX::XMVECTOR m_pFilteredJoints[JointType_Count];
            FilterDoubleExponentialData m_pHistory[JointType_Count];
            FLOAT m_fSmoothing;
            FLOAT m_fCorrection;
            FLOAT m_fPrediction;
            FLOAT m_fJitterRadius;
            FLOAT m_fMaxDeviationRadius;
    
            void Update(Joint joints[], UINT JointID, TRANSFORM_SMOOTH_PARAMETERS smoothingParams);
        };
    }

    //--------------------------------------------------------------------------------------
    // KinectJointFilter.cpp
    //
    // This file contains Holt Double Exponential Smoothing filter for filtering Joints
    //
    // Copyright (C) Microsoft Corporation. All rights reserved.
    //--------------------------------------------------------------------------------------
    
    #include "stdafx.h"
    #include "KinectJointFilter.h"
    
    using namespace Sample;
    using namespace DirectX;
    
    //-------------------------------------------------------------------------------------
    // Name: Lerp()
    // Desc: Linear interpolation between two floats
    //-------------------------------------------------------------------------------------
    inline FLOAT Lerp( FLOAT f1, FLOAT f2, FLOAT fBlend )
    {
        return f1 + ( f2 - f1 ) * fBlend;
    }
     
    //--------------------------------------------------------------------------------------
    // if joint is 0 it is not valid.
    //--------------------------------------------------------------------------------------
    inline BOOL JointPositionIsValid( XMVECTOR vJointPosition )
    {
        return ( XMVectorGetX( vJointPosition ) != 0.0f ||
                    XMVectorGetY( vJointPosition ) != 0.0f ||
                    XMVectorGetZ( vJointPosition ) != 0.0f );
    }
    
    //--------------------------------------------------------------------------------------
    // Implementation of a Holt Double Exponential Smoothing filter. The double exponential
    // smooths the curve and predicts.  There is also noise jitter removal. And maximum
    // prediction bounds.  The paramaters are commented in the init function.
    //--------------------------------------------------------------------------------------
    void FilterDoubleExponential::Update(IBody* const pBody)
    {
        assert( pBody );
    
        // Check for divide by zero. Use an epsilon of a 10th of a millimeter
        m_fJitterRadius = XMMax( 0.0001f, m_fJitterRadius );
    
        TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
        
        UINT jointCapacity = 0;
        Joint joints[JointType_Count];
        
        pBody->GetJoints(jointCapacity, joints);
        for (INT i = 0; i < JointType_Count; i++)
        {
            SmoothingParams.fSmoothing      = m_fSmoothing;
            SmoothingParams.fCorrection     = m_fCorrection;
            SmoothingParams.fPrediction     = m_fPrediction;
            SmoothingParams.fJitterRadius   = m_fJitterRadius;
            SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;
    
            // If inferred, we smooth a bit more by using a bigger jitter radius
            Joint joint = joints[i];
            if ( joint.TrackingState == TrackingState::TrackingState_Inferred )
            {
                SmoothingParams.fJitterRadius       *= 2.0f;
                SmoothingParams.fMaxDeviationRadius *= 2.0f;
            }
    
            Update(joints, i, SmoothingParams);
        }
    }
    
    void FilterDoubleExponential::Update(Joint joints[])
    {
        // Check for divide by zero. Use an epsilon of a 10th of a millimeter
        m_fJitterRadius = XMMax(0.0001f, m_fJitterRadius);
    
        TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
        for (INT i = 0; i < JointType_Count; i++)
        {
            SmoothingParams.fSmoothing = m_fSmoothing;
            SmoothingParams.fCorrection = m_fCorrection;
            SmoothingParams.fPrediction = m_fPrediction;
            SmoothingParams.fJitterRadius = m_fJitterRadius;
            SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;
    
            // If inferred, we smooth a bit more by using a bigger jitter radius
            Joint joint = joints[i];
            if (joint.TrackingState == TrackingState::TrackingState_Inferred)
            {
                SmoothingParams.fJitterRadius *= 2.0f;
                SmoothingParams.fMaxDeviationRadius *= 2.0f;
            }
    
            Update(joints, i, SmoothingParams);
        }
    
    }
    
    void FilterDoubleExponential::Update(Joint joints[], UINT JointID, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
    {
        XMVECTOR vPrevRawPosition;
        XMVECTOR vPrevFilteredPosition;
        XMVECTOR vPrevTrend;
        XMVECTOR vRawPosition;
        XMVECTOR vFilteredPosition;
        XMVECTOR vPredictedPosition;
        XMVECTOR vDiff;
        XMVECTOR vTrend;
        XMVECTOR vLength;
        FLOAT fDiff;
        BOOL bJointIsValid;
    
        const Joint joint = joints[JointID];
    
        vRawPosition = XMVectorSet(joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f);
        vPrevFilteredPosition = m_pHistory[JointID].m_vFilteredPosition;
        vPrevTrend = m_pHistory[JointID].m_vTrend;
        vPrevRawPosition = m_pHistory[JointID].m_vRawPosition;
        bJointIsValid = JointPositionIsValid(vRawPosition);
    
        // If joint is invalid, reset the filter
        if (!bJointIsValid)
        {
            m_pHistory[JointID].m_dwFrameCount = 0;
        }
    
        // Initial start values
        if (m_pHistory[JointID].m_dwFrameCount == 0)
        {
            vFilteredPosition = vRawPosition;
            vTrend = XMVectorZero();
            m_pHistory[JointID].m_dwFrameCount++;
        }
        else if (m_pHistory[JointID].m_dwFrameCount == 1)
        {
            vFilteredPosition = XMVectorScale(XMVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
            vDiff = XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
            vTrend = XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
            m_pHistory[JointID].m_dwFrameCount++;
        }
        else
        {
            // First apply jitter filter
            vDiff = XMVectorSubtract(vRawPosition, vPrevFilteredPosition);
            vLength = XMVector3Length(vDiff);
            fDiff = fabs(XMVectorGetX(vLength));
    
            if (fDiff <= smoothingParams.fJitterRadius)
            {
                vFilteredPosition = XMVectorAdd(XMVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
                    XMVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
            }
            else
            {
                vFilteredPosition = vRawPosition;
            }
    
            // Now the double exponential smoothing filter
            vFilteredPosition = XMVectorAdd(XMVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
                XMVectorScale(XMVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
    
    
            vDiff = XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
            vTrend = XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
        }
    
        // Predict into the future to reduce latency
        vPredictedPosition = XMVectorAdd(vFilteredPosition, XMVectorScale(vTrend, smoothingParams.fPrediction));
    
        // Check that we are not too far away from raw data
        vDiff = XMVectorSubtract(vPredictedPosition, vRawPosition);
        vLength = XMVector3Length(vDiff);
        fDiff = fabs(XMVectorGetX(vLength));
    
        if (fDiff > smoothingParams.fMaxDeviationRadius)
        {
            vPredictedPosition = XMVectorAdd(XMVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
                XMVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
        }
    
        // Save the data from this frame
        m_pHistory[JointID].m_vRawPosition = vRawPosition;
        m_pHistory[JointID].m_vFilteredPosition = vFilteredPosition;
        m_pHistory[JointID].m_vTrend = vTrend;
    
        // Output the data
        m_pFilteredJoints[JointID] = vPredictedPosition;
        m_pFilteredJoints[JointID] = XMVectorSetW(m_pFilteredJoints[JointID], 1.0f);
    }


    Carmine Sirignano - MSFT





    Wednesday, August 20, 2014 7:09 PM