add: 添加平均滤波算法

Assets/Models/PoseLandmarkType.cs

@@ -35,6 +35,7 @@ namespace Models
         LeftHeel,
         RightHeel,
         LeftFootIndex,
-        RightFootIndex
+        RightFootIndex,
+        MaxValue
     }
 }

Assets/Models/PoseTransform.cs

@@ -19,9 +19,14 @@ namespace Models
         public readonly HumanBodyBones UnityName;
 
         /// <summary>
-        /// MediaPipe捕捉空间坐标
+        /// 取平均的长度
         /// </summary>
-        public Vector3 MediaPipePos;
+        public static int AverageLength;
+
+        /// <summary>
+        /// 取平均之后的结果
+        /// </summary>
+        public Vector3 AveragePos;
 
         /// <summary>
         /// 节点的父节点列表
@@ -38,27 +43,52 @@ namespace Models
         /// </summary>
         public Quaternion CurrentQuaternion;
 
+        private int _count;
+        private readonly Queue<PoseLandmark> _landmarkQueue;
+
         public PoseTransform(
-            PoseLandmarkType type
+            PoseLandmarkType type,
+            int averageLength = 5
         )
         {
             MediaPipeName = type;
             UnityName = GetRelatedBone(type);
-            MediaPipePos = new Vector3();
+            AveragePos = new Vector3();
             Parent = new List<PoseTransform>();
             PreviousQuaternion = new Quaternion();
             CurrentQuaternion = new Quaternion();
+            AverageLength = averageLength;
+            
+            _count = 0;
+            _landmarkQueue = new Queue<PoseLandmark>();
         }
 
-        /// <summary>
-        /// 计算当前的旋转四元数
-        /// </summary>
-        /// <param name="landmark"></param>
-        public void CalculateCurrentQuaternion(PoseLandmark landmark)
+        public static void UpdatePosition(ref PoseTransform pose, PoseLandmark landmark)
         {
-            MediaPipePos.x = landmark.X;
-            MediaPipePos.y = landmark.Y;
-            MediaPipePos.z = landmark.Z;
+            if (pose._count < AverageLength)
+            {
+                pose._landmarkQueue.Enqueue(landmark);
+                pose._count++;
+            }
+            else
+            {
+                pose._landmarkQueue.Dequeue();
+                pose._landmarkQueue.Enqueue(landmark);
+
+                var sum = new Vector3();
+
+                foreach (var poseLandmark in pose._landmarkQueue)
+                {
+                    sum += new Vector3(poseLandmark.X, poseLandmark.Y, poseLandmark.Z);
+                }
+
+                pose.AveragePos = sum / AverageLength;
+            }
+        }
+
+        public static void CalculateRotation(PoseTransform poseTransform)
+        {
+            
         }
 
         /// <summary>