之前在微软论坛看到的,转过来。这种算法用于平滑Kinect 2中骨骼点的坐标,具体骨骼点平滑算法原理是什么,我没有深究,又懂的人欢迎留言。
修正了一处c++错误,导致无法得到骨骼数据—-2015-5-19
C++代码
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 |
//-------------------------------------------------------------------------------------- // 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); }; } |
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 |
//-------------------------------------------------------------------------------------- // 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; 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); } |
C#代码
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 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 |
//-------------------------------------------------------------------------------------- // KinectJointFilter.cs // // This file contains Holt Double Exponential Smoothing filter for filtering Joints // // Copyright (C) Microsoft Corporation. All rights reserved. //-------------------------------------------------------------------------------------- using System.Collections; using Windows.Kinect; public class KinectJointFilter { public struct TRANSFORM_SMOOTH_PARAMETERS { public float fSmoothing; // [0..1], lower values closer to raw data public float fCorrection; // [0..1], lower values slower to correct towards the raw data public float fPrediction; // [0..n], the number of frames to predict into the future public float fJitterRadius; // The radius in meters for jitter reduction public float fMaxDeviationRadius; // The maximum radius in meters that filtered positions are allowed to deviate from raw data } public class FilterDoubleExponentialData { public CameraSpacePoint m_vRawPosition; public CameraSpacePoint m_vFilteredPosition; public CameraSpacePoint m_vTrend; public int m_dwFrameCount; } // Holt Double Exponential Smoothing filter CameraSpacePoint[] m_pFilteredJoints; FilterDoubleExponentialData[] m_pHistory; float m_fSmoothing; float m_fCorrection; float m_fPrediction; float m_fJitterRadius; float m_fMaxDeviationRadius; public KinectJointFilter() { m_pFilteredJoints = new CameraSpacePoint[Body.JointCount]; m_pHistory = new FilterDoubleExponentialData[Body.JointCount]; for (int i = 0; i < Body.JointCount; i++) { m_pHistory[i] = new FilterDoubleExponentialData(); } Init(); } ~KinectJointFilter() { Shutdown(); } public 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); } public void Shutdown() { } public void Reset(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f) { if (m_pFilteredJoints == null || m_pHistory == null) { return; } 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 for (int i = 0; i < Body.JointCount; i++) { m_pFilteredJoints[i].X = 0.0f; m_pFilteredJoints[i].Y = 0.0f; m_pFilteredJoints[i].Z = 0.0f; m_pHistory[i].m_vFilteredPosition.X = 0.0f; m_pHistory[i].m_vFilteredPosition.Y = 0.0f; m_pHistory[i].m_vFilteredPosition.Z = 0.0f; m_pHistory[i].m_vRawPosition.X = 0.0f; m_pHistory[i].m_vRawPosition.Y = 0.0f; m_pHistory[i].m_vRawPosition.Z = 0.0f; m_pHistory[i].m_vTrend.X = 0.0f; m_pHistory[i].m_vTrend.Y = 0.0f; m_pHistory[i].m_vTrend.Z = 0.0f; m_pHistory[i].m_dwFrameCount = 0; } } //-------------------------------------------------------------------------------------- // 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. //-------------------------------------------------------------------------------------- public void UpdateFilter(Body pBody) { if (pBody == null) { return; } // Check for divide by zero. Use an epsilon of a 10th of a millimeter m_fJitterRadius = Mathf.Max( 0.0001f, m_fJitterRadius ); TRANSFORM_SMOOTH_PARAMETERS SmoothingParams; for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++) { 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 Windows.Kinect.Joint joint = pBody.Joints[jt]; if ( joint.TrackingState == TrackingState.Inferred ) { SmoothingParams.fJitterRadius *= 2.0f; SmoothingParams.fMaxDeviationRadius *= 2.0f; } UpdateJoint(pBody, jt, SmoothingParams); } } //-------------------------------------------------------------------------------------- // if joint is 0 it is not valid. //-------------------------------------------------------------------------------------- bool JointPositionIsValid( CameraSpacePoint vJointPosition ) { return ( vJointPosition.X != 0.0f || vJointPosition.Y != 0.0f || vJointPosition.Z != 0.0f ); } public CameraSpacePoint[] GetFilteredJoints() { return m_pFilteredJoints; } CameraSpacePoint CSVectorSet(float x, float y, float z) { CameraSpacePoint point = new CameraSpacePoint(); point.X = x; point.Y = y; point.Z = z; return point; } CameraSpacePoint CSVectorZero() { CameraSpacePoint point = new CameraSpacePoint(); point.X = 0.0f; point.Y = 0.0f; point.Z = 0.0f; return point; } CameraSpacePoint CSVectorAdd(CameraSpacePoint p1, CameraSpacePoint p2) { CameraSpacePoint sum = new CameraSpacePoint(); sum.X = p1.X + p2.X; sum.Y = p1.Y + p2.Y; sum.Z = p1.Z + p2.Z; return sum; } CameraSpacePoint CSVectorScale(CameraSpacePoint p, float scale) { CameraSpacePoint point = new CameraSpacePoint(); point.X = p.X * scale; point.Y = p.Y * scale; point.Z = p.Z * scale; return point; } CameraSpacePoint CSVectorSubtract(CameraSpacePoint p1, CameraSpacePoint p2) { CameraSpacePoint diff = new CameraSpacePoint(); diff.X = p1.X - p2.X; diff.Y = p1.Y - p2.Y; diff.Z = p1.Z - p2.Z; return diff; } float CSVectorLength(CameraSpacePoint p) { return Mathf.Sqrt(p.X * p.X + p.Y * p.Y + p.Z * p.Z); } 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; } } |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
on setup: create an instance of the class, one for each body if you want to filter more bodies: KinectJointFilter filter = new KinectJointFilter(); filter.Init(); // change params if you want Whenever you get a body frame update: // get a valid and tracked body // pass to the filter object filter.UpdateFilter(body) whenever you want the filtered Joints, the function will return a collection. CameraSpacePoints filteredJoints[] = filter.GetFilteredJoints(); If you don't want all the joints, you could probably optimize and add a function where you pass in the joint and it will return the position only. |