UnaNancyOwen/Body.cpp

## Body.cpp
// Body.cpp : コンソール アプリケーションのエントリ ポイントを定義します。
// This source code is licensed under the MIT license. Please see the License in License.txt.
// "This is preliminary software and/or hardware and APIs are preliminary and subject to change."
//

#include "stdafx.h"
#include <Windows.h>
#include <Kinect.h>
#include "KinectJointFilter.h"
#include <opencv2/opencv.hpp>


template<class Interface>
inline void SafeRelease( Interface *& pInterfaceToRelease )
{
	if( pInterfaceToRelease != NULL ){
		pInterfaceToRelease->Release();
		pInterfaceToRelease = NULL;
	}
}

int _tmain( int argc, _TCHAR* argv[] )
{
	cv::setUseOptimized( true );

	// Sensor
	IKinectSensor* pSensor;
	HRESULT hResult = S_OK;
	hResult = GetDefaultKinectSensor( &pSensor );
	if( FAILED( hResult ) ){
		std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
		return -1;
	}

	hResult = pSensor->Open( );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IKinectSensor::Open()" << std::endl;
		return -1;
	}

	// Source
	IColorFrameSource* pColorSource;
	hResult = pSensor->get_ColorFrameSource( &pColorSource );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
		return -1;
	}

	IBodyFrameSource* pBodySource;
	hResult = pSensor->get_BodyFrameSource( &pBodySource );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IKinectSensor::get_BodyFrameSource()" << std::endl;
		return -1;
	}

	// Reader
	IColorFrameReader* pColorReader;
	hResult = pColorSource->OpenReader( &pColorReader );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
		return -1;
	}

	IBodyFrameReader* pBodyReader;
	hResult = pBodySource->OpenReader( &pBodyReader );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IBodyFrameSource::OpenReader()" << std::endl;
		return -1;
	}

	// Coordinate Mapper
	ICoordinateMapper* pCoordinateMapper;
	hResult = pSensor->get_CoordinateMapper( &pCoordinateMapper );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
		return -1;
	}

	// Description
	IFrameDescription* pDescription;
	hResult = pColorSource->get_FrameDescription( &pDescription );
	if( FAILED( hResult ) ){
		std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
		return -1;
	}
	int width = 0;
	int height = 0;
	pDescription->get_Width( &width ); // 1920
	pDescription->get_Height( &height ); // 1080
	unsigned int bufferSize = width * height * 4 * sizeof( unsigned char );

	cv::Mat bufferMat( height, width, CV_8UC4 );
	cv::Mat colorMat( height / 2, width / 2, CV_8UC4 );

	// Create Window
	cv::namedWindow( "Body" );

	// Color Table
	cv::Vec3b color[BODY_COUNT];
	color[0] = cv::Vec3b( 255,   0,   0 );
	color[1] = cv::Vec3b(   0, 255,   0 );
	color[2] = cv::Vec3b(   0,   0, 255 );
	color[3] = cv::Vec3b( 255, 255,   0 );
	color[4] = cv::Vec3b( 255,   0, 255 );
	color[5] = cv::Vec3b(   0, 255, 255 );

	// Holt Double Exponential Smoothing Filter
	Sample::FilterDoubleExponential filter[BODY_COUNT];

	// Option : Setting Smoothing Parameter
	for( int count = 0; count < BODY_COUNT; count++ ){
		float smoothing = 0.25f;          // [0..1], lower values closer to raw data
		float correction = 0.25f;         // [0..1], lower values slower to correct towards the raw data
		float prediction = 0.25f;         // [0..n], the number of frames to predict into the future
		float jitterRadius = 0.03f;       // The radius in meters for jitter reduction
		float maxDeviationRadius = 0.05f; // The maximum radius in meters that filtered positions are allowed to deviate from raw data

		filter[count].Init( smoothing, correction, prediction, jitterRadius, maxDeviationRadius );
	}

	while( 1 ){
		// Color Frame
		IColorFrame* pColorFrame = nullptr;
		hResult = pColorReader->AcquireLatestFrame( &pColorFrame );
		if( SUCCEEDED( hResult ) ){
			hResult = pColorFrame->CopyConvertedFrameDataToArray( bufferSize, reinterpret_cast<BYTE*>( bufferMat.data ), ColorImageFormat::ColorImageFormat_Bgra );
			if( SUCCEEDED( hResult ) ){
				cv::resize( bufferMat, colorMat, cv::Size(), 0.5, 0.5 );
			}
		}
		//SafeRelease( pColorFrame );

		// Body Frame
		IBodyFrame* pBodyFrame = nullptr;
		hResult = pBodyReader->AcquireLatestFrame( &pBodyFrame );
		if( SUCCEEDED( hResult ) ){
			IBody* pBody[BODY_COUNT] = { 0 };
			hResult = pBodyFrame->GetAndRefreshBodyData( BODY_COUNT, pBody );
			if( SUCCEEDED( hResult ) ){
				for( int count = 0; count < BODY_COUNT; count++ ){
					BOOLEAN bTracked = false;
					hResult = pBody[count]->get_IsTracked( &bTracked );
					if( SUCCEEDED( hResult ) && bTracked ){
						Joint joint[JointType::JointType_Count];
						hResult = pBody[count]->GetJoints( JointType::JointType_Count, joint );
						if( SUCCEEDED( hResult ) ){
							// Raw Joint
							for( int type = 0; type < JointType::JointType_Count; type++ ){
								if( joint[type].TrackingState != TrackingState::TrackingState_NotTracked ){
									ColorSpacePoint colorSpacePoint = { 0 };
									pCoordinateMapper->MapCameraPointToColorSpace( joint[type].Position, &colorSpacePoint );
									int colorX = static_cast<int>( colorSpacePoint.X + 0.5f );
									int colorY = static_cast<int>( colorSpacePoint.Y + 0.5f );
									if( ( colorX >= 0 ) && ( colorX < width ) && ( colorY >= 0 ) && ( colorY < height ) ){
										cv::circle( bufferMat, cv::Point( colorX, colorY ), 5, static_cast<cv::Scalar>( color[count] ), -1, CV_AA );
									}
								}
							}

							// Filtered Joint
							filter[count].Update( joint );
							const DirectX::XMVECTOR* vec = filter[count].GetFilteredJoints();
							for( int type = 0; type < JointType::JointType_Count; type++ ){
								if( joint[type].TrackingState != TrackingState::TrackingState_NotTracked ){
									float x = 0.0f, y = 0.0f, z = 0.0f;
									DirectX::XMVectorGetXPtr( &x, vec[type] );
									DirectX::XMVectorGetYPtr( &y, vec[type] );
									DirectX::XMVectorGetZPtr( &z, vec[type] );

									ColorSpacePoint colorSpacePoint = { 0 };
									CameraSpacePoint cameraSpacePoint = { x, y, z };
									pCoordinateMapper->MapCameraPointToColorSpace( cameraSpacePoint, &colorSpacePoint );
									int colorX = static_cast<int>( colorSpacePoint.X + 0.5f );
									int colorY = static_cast<int>( colorSpacePoint.Y + 0.5f );
									if( ( colorX >= 0 ) && ( colorX < width ) && ( colorY >= 0 ) && ( colorY < height ) ){
										cv::circle( bufferMat, cv::Point( colorX, colorY ), 5, static_cast<cv::Scalar>( color[count] ), -1, CV_AA );
									}
								}
							}
						}
					}
				}
				cv::resize( bufferMat, colorMat, cv::Size(), 0.5, 0.5 );
			}
			for( int count = 0; count < BODY_COUNT; count++ ){
				SafeRelease( pBody[count] );
			}
		}

		SafeRelease( pColorFrame );
		SafeRelease( pBodyFrame );

		// Show Image
		cv::imshow( "Body", colorMat );

		// Wait Key
		if( cv::waitKey( 10 ) == VK_ESCAPE ){
			break;
		}
	}

	// Release Memory
	SafeRelease( pColorSource );
	SafeRelease( pBodySource );
	SafeRelease( pColorReader );
	SafeRelease( pBodyReader );
	SafeRelease( pDescription );
	SafeRelease( pCoordinateMapper );
	if( pSensor ){
		pSensor->Close();
	}
	SafeRelease( pSensor );
	cv::destroyAllWindows();

	return 0;
}

## KinectJointFilter.cpp
//--------------------------------------------------------------------------------------
// 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 );
}

## KinectJointFilter.h
//--------------------------------------------------------------------------------------
// KinectJointFilter.h
//
// This file contains Holt Double Exponential Smoothing filter for filtering Joints
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------

#pragma once

#include <Windows.h>
#include <Kinect.h>
#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 );
	};
}
	// Body.cpp : コンソールアプリケーションのエントリポイントを定義します。
	// This source code is licensed under the MIT license. Please see the License in License.txt.
	// "This is preliminary software and/or hardware and APIs are preliminary and subject to change."
	//

	#include "stdafx.h"
	#include <Windows.h>
	#include <Kinect.h>
	#include "KinectJointFilter.h"
	#include <opencv2/opencv.hpp>


	template<class Interface>
	inline void SafeRelease( Interface *& pInterfaceToRelease )
	{
	if( pInterfaceToRelease != NULL ){
	pInterfaceToRelease->Release();
	pInterfaceToRelease = NULL;
	}
	}

	int _tmain( int argc, _TCHAR* argv[] )
	{
	cv::setUseOptimized( true );

	// Sensor
	IKinectSensor* pSensor;
	HRESULT hResult = S_OK;
	hResult = GetDefaultKinectSensor( &pSensor );
	if( FAILED( hResult ) ){
	std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
	return -1;
	}

	hResult = pSensor->Open( );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IKinectSensor::Open()" << std::endl;
	return -1;
	}

	// Source
	IColorFrameSource* pColorSource;
	hResult = pSensor->get_ColorFrameSource( &pColorSource );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
	return -1;
	}

	IBodyFrameSource* pBodySource;
	hResult = pSensor->get_BodyFrameSource( &pBodySource );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IKinectSensor::get_BodyFrameSource()" << std::endl;
	return -1;
	}

	// Reader
	IColorFrameReader* pColorReader;
	hResult = pColorSource->OpenReader( &pColorReader );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
	return -1;
	}

	IBodyFrameReader* pBodyReader;
	hResult = pBodySource->OpenReader( &pBodyReader );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IBodyFrameSource::OpenReader()" << std::endl;
	return -1;
	}

	// Coordinate Mapper
	ICoordinateMapper* pCoordinateMapper;
	hResult = pSensor->get_CoordinateMapper( &pCoordinateMapper );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
	return -1;
	}

	// Description
	IFrameDescription* pDescription;
	hResult = pColorSource->get_FrameDescription( &pDescription );
	if( FAILED( hResult ) ){
	std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
	return -1;
	}
	int width = 0;
	int height = 0;
	pDescription->get_Width( &width ); // 1920
	pDescription->get_Height( &height ); // 1080
	unsigned int bufferSize = width * height * 4 * sizeof( unsigned char );

	cv::Mat bufferMat( height, width, CV_8UC4 );
	cv::Mat colorMat( height / 2, width / 2, CV_8UC4 );

	// Create Window
	cv::namedWindow( "Body" );

	// Color Table
	cv::Vec3b color[BODY_COUNT];
	color[0] = cv::Vec3b( 255, 0, 0 );
	color[1] = cv::Vec3b( 0, 255, 0 );
	color[2] = cv::Vec3b( 0, 0, 255 );
	color[3] = cv::Vec3b( 255, 255, 0 );
	color[4] = cv::Vec3b( 255, 0, 255 );
	color[5] = cv::Vec3b( 0, 255, 255 );

	// Holt Double Exponential Smoothing Filter
	Sample::FilterDoubleExponential filter[BODY_COUNT];

	// Option : Setting Smoothing Parameter
	for( int count = 0; count < BODY_COUNT; count++ ){
	float smoothing = 0.25f; // [0..1], lower values closer to raw data
	float correction = 0.25f; // [0..1], lower values slower to correct towards the raw data
	float prediction = 0.25f; // [0..n], the number of frames to predict into the future
	float jitterRadius = 0.03f; // The radius in meters for jitter reduction
	float maxDeviationRadius = 0.05f; // The maximum radius in meters that filtered positions are allowed to deviate from raw data

	filter[count].Init( smoothing, correction, prediction, jitterRadius, maxDeviationRadius );
	}

	while( 1 ){
	// Color Frame
	IColorFrame* pColorFrame = nullptr;
	hResult = pColorReader->AcquireLatestFrame( &pColorFrame );
	if( SUCCEEDED( hResult ) ){
	hResult = pColorFrame->CopyConvertedFrameDataToArray( bufferSize, reinterpret_cast<BYTE*>( bufferMat.data ), ColorImageFormat::ColorImageFormat_Bgra );
	if( SUCCEEDED( hResult ) ){
	cv::resize( bufferMat, colorMat, cv::Size(), 0.5, 0.5 );
	}
	}
	//SafeRelease( pColorFrame );

	// Body Frame
	IBodyFrame* pBodyFrame = nullptr;
	hResult = pBodyReader->AcquireLatestFrame( &pBodyFrame );
	if( SUCCEEDED( hResult ) ){
	IBody* pBody[BODY_COUNT] = { 0 };
	hResult = pBodyFrame->GetAndRefreshBodyData( BODY_COUNT, pBody );
	if( SUCCEEDED( hResult ) ){
	for( int count = 0; count < BODY_COUNT; count++ ){
	BOOLEAN bTracked = false;
	hResult = pBody[count]->get_IsTracked( &bTracked );
	if( SUCCEEDED( hResult ) && bTracked ){
	Joint joint[JointType::JointType_Count];
	hResult = pBody[count]->GetJoints( JointType::JointType_Count, joint );
	if( SUCCEEDED( hResult ) ){
	// Raw Joint
	for( int type = 0; type < JointType::JointType_Count; type++ ){
	if( joint[type].TrackingState != TrackingState::TrackingState_NotTracked ){
	ColorSpacePoint colorSpacePoint = { 0 };
	pCoordinateMapper->MapCameraPointToColorSpace( joint[type].Position, &colorSpacePoint );
	int colorX = static_cast<int>( colorSpacePoint.X + 0.5f );
	int colorY = static_cast<int>( colorSpacePoint.Y + 0.5f );
	if( ( colorX >= 0 ) && ( colorX < width ) && ( colorY >= 0 ) && ( colorY < height ) ){
	cv::circle( bufferMat, cv::Point( colorX, colorY ), 5, static_cast<cv::Scalar>( color[count] ), -1, CV_AA );
	}
	}
	}

	// Filtered Joint
	filter[count].Update( joint );
	const DirectX::XMVECTOR* vec = filter[count].GetFilteredJoints();
	for( int type = 0; type < JointType::JointType_Count; type++ ){
	if( joint[type].TrackingState != TrackingState::TrackingState_NotTracked ){
	float x = 0.0f, y = 0.0f, z = 0.0f;
	DirectX::XMVectorGetXPtr( &x, vec[type] );
	DirectX::XMVectorGetYPtr( &y, vec[type] );
	DirectX::XMVectorGetZPtr( &z, vec[type] );

	ColorSpacePoint colorSpacePoint = { 0 };
	CameraSpacePoint cameraSpacePoint = { x, y, z };
	pCoordinateMapper->MapCameraPointToColorSpace( cameraSpacePoint, &colorSpacePoint );
	int colorX = static_cast<int>( colorSpacePoint.X + 0.5f );
	int colorY = static_cast<int>( colorSpacePoint.Y + 0.5f );
	if( ( colorX >= 0 ) && ( colorX < width ) && ( colorY >= 0 ) && ( colorY < height ) ){
	cv::circle( bufferMat, cv::Point( colorX, colorY ), 5, static_cast<cv::Scalar>( color[count] ), -1, CV_AA );
	}
	}
	}
	}
	}
	}
	cv::resize( bufferMat, colorMat, cv::Size(), 0.5, 0.5 );
	}
	for( int count = 0; count < BODY_COUNT; count++ ){
	SafeRelease( pBody[count] );
	}
	}

	SafeRelease( pColorFrame );
	SafeRelease( pBodyFrame );

	// Show Image
	cv::imshow( "Body", colorMat );

	// Wait Key
	if( cv::waitKey( 10 ) == VK_ESCAPE ){
	break;
	}
	}

	// Release Memory
	SafeRelease( pColorSource );
	SafeRelease( pBodySource );
	SafeRelease( pColorReader );
	SafeRelease( pBodyReader );
	SafeRelease( pDescription );
	SafeRelease( pCoordinateMapper );
	if( pSensor ){
	pSensor->Close();
	}
	SafeRelease( pSensor );
	cv::destroyAllWindows();

	return 0;
	}
	//--------------------------------------------------------------------------------------
	// 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 );
	}
	//--------------------------------------------------------------------------------------
	// KinectJointFilter.h
	//
	// This file contains Holt Double Exponential Smoothing filter for filtering Joints
	//
	// Copyright (C) Microsoft Corporation. All rights reserved.
	//--------------------------------------------------------------------------------------

	#pragma once

	#include <Windows.h>
	#include <Kinect.h>
	#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 );
	};
	}
Parameter	Range	Default	Explanation
fSmoothing	[0.0f-1.0f]	0.25f	Smoothing wait. The lower values closer to raw data.
fCorrection	[0.0f-1.0f]	0.25f	Correction amount. The lower values, slower to correct towards the raw data.
fPrediction	[0.0f-]	0.25f	Predict frames. The number of frames to predict into the future.
fJitterRadius	[0.0f-]	0.03f	Jitter clamp radius. The radius in meters for jitter reduction. [m]
fMaxDeviationRadius	[0.0f-]	0.05f	Maximum radius to clamp. The maximum radius in meters that filtered positions are allowed to deviate from raw data. [m]