-
Notifications
You must be signed in to change notification settings - Fork 1
/
KinectJointFilter.cpp
191 lines (158 loc) · 6.72 KB
/
KinectJointFilter.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
//--------------------------------------------------------------------------------------
// KinectJointFilter.cpp
//
// This file contains Holt Double Exponential Smoothing filter for filtering Joints
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------
#include <iostream>
#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;
Joint joints[JointType_Count];
pBody->GetJoints(_countof(joints), 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);
}