add: 添加平均滤波算法

Assets/MainBehaviour.cs

@@ -4,16 +4,17 @@
 
 public class MainBehaviour : MonoBehaviour
 {
-    private readonly List<GameObject> _nodes = new List<GameObject>();
+    private readonly GameObject[] _nodes = new GameObject[(int)PoseLandmarkType.MaxValue];
     private readonly List<Bond> _bonds = new List<Bond>();
-    private readonly List<VirtualSkeleton> _virtualSkeletons = new List<VirtualSkeleton>();
+    private readonly PoseTransform[] _poseTransforms = new PoseTransform[(int)PoseLandmarkType.MaxValue];
-    private readonly List<PoseTransform> _poseTransforms = new List<PoseTransform>();
     private readonly UdpListener _listener = new UdpListener();
     private const float Scale = 0.2f;
 
     // Start is called before the first frame update
     private void Start()
     {
+        PoseTransform.AverageLength = 3;
+
         CreateNodes();
         CreateBonds();
 
@@ -24,25 +25,15 @@ private void Start()
     // Update is called once per frame
     private void Update()
     {
-        for (var i = 0; i < 33; i++)
+        for (var i = 0; i < (int)PoseLandmarkType.MaxValue; i++)
         {
-            var temp = new Vector4(_poseTransforms[i].MediaPipePos.x,
+            _nodes[i].transform.position = _poseTransforms[i].AveragePos * -5;
-                _poseTransforms[i].MediaPipePos.y,
-                _poseTransforms[i].MediaPipePos.z,
-                1);
-
-            _nodes[i].transform.position = new Vector3(-temp.x, -temp.y, -temp.z) * 5;
         }
 
         foreach (var bond in _bonds)
         {
             bond.UpdateBond();
         }
-
-        /*foreach (var vs in _virtualSkeletons)
-        {
-            vs.UpdateVS();
-        }*/
     }
 
     private void OnDisable()
@@ -50,9 +41,17 @@ private void OnDisable()
         _listener.DisConnect();
     }
     
+    private void OnReceive(List<PoseLandmark> landmarks)
+    {
+        foreach (var landmark in landmarks)
+        {
+            PoseTransform.UpdatePosition(ref _poseTransforms[(int)landmark.Type], landmark);
+        }
+    }
+
     private void CreateNodes()
     {
-        for (var i = 0; i < 33; i++)
+        for (var i = 0; i < (int)PoseLandmarkType.MaxValue; i++)
         {
             var ball = GameObject.CreatePrimitive(PrimitiveType.Sphere);
 
@@ -71,11 +70,8 @@ private void CreateNodes()
                 ball.GetComponent<Renderer>().material.color = Color.green;//给脚部添加颜色
             }
 
-            _nodes.Add(ball);
+            _nodes[i] = ball;
-
+            _poseTransforms[i] = new PoseTransform((PoseLandmarkType)i);
-            var poseTransform = new PoseTransform((PoseLandmarkType)i);
-            
-            _poseTransforms.Add(poseTransform);
         }
     }
 
@@ -161,19 +157,6 @@ private void CreateBonds()
             Scale));
     }
 
-    private void OnReceive(List<PoseLandmark> landmarks)
-    {
-        foreach (var landmark in landmarks)
-        {
-            var poseTransform = new PoseTransform(landmark.Type)
-            {
-                MediaPipePos = new Vector3(landmark.X, landmark.Y, landmark.Z)
-            };
-
-            _poseTransforms[(int)landmark.Type] = poseTransform;
-        }
-    }
-
     /// <summary>
     /// 创建棍子列表
     /// </summary>

Assets/ModelBehaviour.cs

@@ -58,31 +58,7 @@ private static void RigPoint(List<PoseLandmark> landmarks)
         
         foreach (var landmark in landmarks)
         {
-            PoseTransforms[(int)landmark.Type].CalculateCurrentQuaternion(landmark);
             
-            
-            // var temp = _posLandmarks.Find(t=>t.mediaPipeName == landmark.Type.ToString());
-            // var sum = new Vector3(0,0,0);
-            //var mediatemp = new Vector3(landmark.X, landmark.Y, landmark.Z);
-            //temp.mediaPipePos = new Vector3(landmark.X, landmark.Y, landmark.Z);
-            //temp.mediaPipePos.x = landmark.X;
-            //temp.mediaPipePos.y = landmark.Y;
-            //temp.mediaPipePos.z = landmark.Z;
-            //foreach (var parents in temp.parent)
-            //{
-            //    if (parents != null)
-            //   {
-            //       sum += parents.mediaPipePos;
-            //   }
-
-
-            //}
-
-            //var currentPos = sum / temp.parent.Count();
-
-            //temp.currentQ = Quaternion.Euler(temp.mediaPipePos - currentPos);
-
-            //Debug.Log("Here!!");
         }
 
     }

Assets/Models/PoseLandmarkType.cs

@@ -35,6 +35,7 @@ public enum PoseLandmarkType
         LeftHeel,
         RightHeel,
         LeftFootIndex,
-        RightFootIndex
+        RightFootIndex,
+        MaxValue
     }
 }

Assets/Models/PoseTransform.cs

@@ -19,9 +19,14 @@ public struct PoseTransform
         public readonly HumanBodyBones UnityName;
 
         /// <summary>
-        /// MediaPipe捕捉空间坐标
+        /// 取平均的长度
         /// </summary>
-        public Vector3 MediaPipePos;
+        public static int AverageLength;
+
+        /// <summary>
+        /// 取平均之后的结果
+        /// </summary>
+        public Vector3 AveragePos;
 
         /// <summary>
         /// 节点的父节点列表
@@ -38,27 +43,52 @@ public struct PoseTransform
         /// </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>
+        public static void UpdatePosition(ref PoseTransform pose, PoseLandmark landmark)
-        /// 计算当前的旋转四元数
-        /// </summary>
-        /// <param name="landmark"></param>
-        public void CalculateCurrentQuaternion(PoseLandmark landmark)
         {
-            MediaPipePos.x = landmark.X;
+            if (pose._count < AverageLength)
-            MediaPipePos.y = landmark.Y;
+            {
-            MediaPipePos.z = landmark.Z;
+                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>