优化关键点间相对位置判断,统一管理角度

android/app/src/main/java/org/tensorflow/lite/examples/poseestimation/ml/PoseNet.kt

@@ -142,29 +142,6 @@ class PoseNet(private val interpreter: Interpreter, private var gpuDelegate: Gpu
         val X = x2 - x1
         return Math.toDegrees(atan2(Y.toDouble(),X.toDouble())).toFloat()
     }
-    // 函数2: 计算角ABC
-    fun abttPoints(A: KeyPoint, B: KeyPoint, C: KeyPoint): Float {
-        // 计算向量BA和BC的坐标
-        val BAx = A.coordinate.x - B.coordinate.x
-        val BAy = A.coordinate.y - B.coordinate.y
-        val BCx = C.coordinate.x - B.coordinate.x
-        val BCy = C.coordinate.y - B.coordinate.y
-
-        // 计算BA和BC的点积
-        val dotProduct = BAx * BCx + BAy * BCy
-
-        // 计算BA和BC的模长
-        val magBA = sqrt(BAx * BAx + BAy * BAy)
-        val magBC = sqrt(BCx * BCx + BCy * BCy)
-
-        // 计算夹角的cos值
-        val cosAngle = dotProduct / (magBA * magBC)
-
-        // 计算夹角的角度（弧度转角度）
-        val angle = Math.toDegrees(acos(cosAngle.toDouble())).toFloat()
-
-        return angle
-    }
     private fun postProcessModelOuputs(
         heatmaps: Array<Array<Array<FloatArray>>>,
         offsets: Array<Array<Array<FloatArray>>>
@@ -231,10 +208,9 @@ class PoseNet(private val interpreter: Interpreter, private var gpuDelegate: Gpu
         // 在此部分中添加角度数据和合格率数据
         // 遍历 keyPoints
         keypointList.forEach { keypoint ->
-            // 获取当前关键点的位置和置信度
+            // 获取当前关键点的位置
             val x = keypoint.coordinate.x
             val y = keypoint.coordinate.y
-            //这里添加标准度
 
             // 假设我们计算胸部与左右肩膀之间的角度（这里只是示例，具体取决于你的模型）
             if (keypoint.bodyPart == BodyPart.LEFT_SHOULDER || keypoint.bodyPart == BodyPart.RIGHT_SHOULDER) {
@@ -258,6 +234,96 @@ class PoseNet(private val interpreter: Interpreter, private var gpuDelegate: Gpu
         // 返回一个包含关键点和总分数的Person对象
         return Person(keyPoints = keypointList.toList(), score = totalScore / numKeypoints)
     }
+    class KeypointAngles {
+        // 用来存储角度的列表
+        val angles = mutableListOf<Float>()
+
+        // 计算三点之间的夹角
+        fun abttPoints(A: KeyPoint, B: KeyPoint, C: KeyPoint): Float {
+            val BAx = A.coordinate.x - B.coordinate.x
+            val BAy = A.coordinate.y - B.coordinate.y
+            val BCx = C.coordinate.x - B.coordinate.x
+            val BCy = C.coordinate.y - B.coordinate.y
+
+            val dotProduct = BAx * BCx + BAy * BCy
+            val magBA = sqrt(BAx * BAx + BAy * BAy)
+            val magBC = sqrt(BCx * BCx + BCy * BCy)
+
+            val cosAngle = dotProduct / (magBA * magBC)
+            return Math.toDegrees(acos(cosAngle.toDouble())).toFloat()
+        }
+
+        // 添加角度并控制角度列表长度
+        fun addAngle(angle: Float) {
+            angles.add(angle)
+            if (angles.size > 8) {
+                angles.removeAt(0)
+            }
+        }
+
+        // 通用方法：根据三点计算并添加角度
+        fun calculateAndAddAngle(A: KeyPoint, B: KeyPoint, C: KeyPoint) {
+            val angle = abttPoints(A, B, C)
+            addAngle(angle)
+        }
+
+        // 遍历keypointList并计算夹角
+        fun csAngles(keypointList: List<KeyPoint>) {
+            keypointList.forEach { keypoint ->
+                when (keypoint.bodyPart) {
+                    BodyPart.LEFT_SHOULDER -> {
+                        val leftShoulder = keypoint
+                        val leftElbow = keypointList.first { it.bodyPart == BodyPart.LEFT_ELBOW }
+                        val leftWrist = keypointList.first { it.bodyPart == BodyPart.LEFT_WRIST }
+                        calculateAndAddAngle(leftShoulder, leftElbow, leftWrist)
+
+                        val rightShoulder = keypointList.first { it.bodyPart == BodyPart.RIGHT_SHOULDER }
+                        val rightElbow = keypointList.first { it.bodyPart == BodyPart.RIGHT_ELBOW }
+                        calculateAndAddAngle(leftShoulder, rightShoulder, rightElbow)
+                    }
+                    BodyPart.RIGHT_SHOULDER -> {
+                        val rightShoulder = keypoint
+                        val rightElbow = keypointList.first { it.bodyPart == BodyPart.RIGHT_ELBOW }
+                        val rightWrist = keypointList.first { it.bodyPart == BodyPart.RIGHT_WRIST }
+                        calculateAndAddAngle(rightShoulder, rightElbow, rightWrist)
+
+                        val leftShoulder = keypointList.first { it.bodyPart == BodyPart.LEFT_SHOULDER }
+                        val leftElbow = keypointList.first { it.bodyPart == BodyPart.LEFT_ELBOW }
+                        calculateAndAddAngle(rightShoulder, leftShoulder, leftElbow)
+                    }
+                    BodyPart.LEFT_HIP -> {
+                        val leftHip = keypoint
+                        val rightHip = keypointList.first { it.bodyPart == BodyPart.RIGHT_HIP }
+                        val leftKnee = keypointList.first { it.bodyPart == BodyPart.LEFT_KNEE }
+                        calculateAndAddAngle(rightHip, leftHip, leftKnee)
+
+                        val rightKnee = keypointList.first { it.bodyPart == BodyPart.RIGHT_KNEE }
+                        calculateAndAddAngle(leftHip, rightHip, rightKnee)
+
+                        val leftAnkle = keypointList.first { it.bodyPart == BodyPart.LEFT_ANKLE }
+                        calculateAndAddAngle(leftHip, leftKnee, leftAnkle)
+                    }
+                    BodyPart.RIGHT_HIP -> {
+                        val rightHip = keypoint
+                        val leftHip = keypointList.first { it.bodyPart == BodyPart.LEFT_HIP }
+                        val rightKnee = keypointList.first { it.bodyPart == BodyPart.RIGHT_KNEE }
+                        calculateAndAddAngle(rightHip, leftHip, rightKnee)
+
+                        val rightAnkle = keypointList.first { it.bodyPart == BodyPart.RIGHT_ANKLE }
+                        calculateAndAddAngle(rightHip, rightKnee, rightAnkle)
+                    }
+                    // 这里可以继续添加其他 BodyPart 的处理分支
+                    else -> {
+                        // 如果遇到其他没有显式列出的bodyPart，进行处理
+                        println("Unknown body part: ${keypoint.bodyPart}")
+                    }
+                }
+            }
+        }
+    }
+
+
+
 
     // 返回上一次推理的时间
     override fun lastInferenceTimeNanos(): Long = lastInferenceTimeNanos