UnaNancyOwen
12/1/2014 - 4:08 PM
How to use Joint Smoothing
How to use Joint Smoothing
// 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 );
};
}