跌倒检测 关节点角度数学计算

参考：
https://github.com/GitGudwl/MediapipePoseEstimationForFallDetection/tree/main
https://blog.csdn.net/weixin_45824067/article/details/130646962

1、mediapipe 根据关节点角度计算

1、11与12取中间点，记为center_up; 23 与24取中间点记为center_down,作直角三角形，直角为right_angle_point。
2、center_up与center_down连线，right_angle_point与center_down连线，计算这线夹角的tan值，然后用tan的反函数求出夹角值。
3、当夹角>60度，或center_up_y <= center_down_y , 或检测框宽高比大于5/3,则为跌倒

### 这种方法完全躺平状态角度不准，所有可能要统计跌倒过程中如果有fall及表示跌倒
import math
import cv2
import mediapipe as mp
import numpy as np


mp_drawing = mp.solutions.drawing_utils
mp_pose = mp.solutions.pose
# Path to the video file
video_path = "queda.mp4"

cap = cv2.VideoCapture(video_path)

# Curl counter variables
counter = 0
stage = None

# Setup mediapipe instance
mp_pose = mp.solutions.pose
mp_drawing = mp.solutions.drawing_utils
with mp_pose.Pose(min_detection_confidence=0.7, min_tracking_confidence=0.7) as pose:
    while cap.isOpened():
        ret, frame = cap.read()
        if not ret:
            break

        # Recolor image to RGB
        image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        image.flags.writeable = False

        # Make detection
        results = pose.process(image)

        # Recolor back to BGR
        image.flags.writeable = True
        image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)

        # Extract landmarks
        # try:
        if results.pose_landmarks is not None:
            landmarks = results.pose_landmarks.landmark
            pt5 =  (landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value].x,landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value].y)
            pt6 = (landmarks[mp_pose.PoseLandmark.RIGHT_SHOULDER.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_SHOULDER.value].y)
            pt11 = (landmarks[mp_pose.PoseLandmark.LEFT_HIP.value].x,landmarks[mp_pose.PoseLandmark.LEFT_HIP.value].y)
            pt12 =  (landmarks[mp_pose.PoseLandmark.RIGHT_HIP.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_HIP.value].y)
            # 计算 pt5 和 pt6 的中点和 pt11 和 pt12 的中点
            center_up = ((pt5[0]+pt6[0])/2, (pt5[1]+pt6[1])/2)
            center_down = ((pt11[0]+pt12[0])/2, (pt11[1]+pt12[1])/2)

            # 计算直角三角形的直角点
            right_angle_point = center_down[0], center_up[1]

            # 计算 center_up 和 center_down 连线与 right_angle_point 和 center_down 连线之间的夹角
            dx1 = right_angle_point[0] - center_down[0]
            dy1 = right_angle_point[1] - center_down[1]
            dx2 = center_up[0] - center_down[0]
            dy2 = center_up[1] - center_down[1]
            angle = math.atan2(dy1*dx2 - dx1*dy2, dx1*dx2 + dy1*dy2)

            # 将弧度转换为角度
            angle = angle * 180 / math.pi

            # 判断夹角是否大于 60 度
            if angle > 60:
                key = "fall"
                print("跌倒了")
            else:
                key = "up"
                print("未跌倒")

            # Rep data
            cv2.putText(image, key+"  "+str(angle), (12,35),
                    cv2.FONT_HERSHEY_SIMPLEX, 2, (255,0,0), 3, cv2.LINE_AA)
            
                # Render detections
            mp_drawing.draw_landmarks(image, results.pose_landmarks, mp_pose.POSE_CONNECTIONS,
                                    mp_drawing.DrawingSpec(color=(245,117,66), thickness=2, circle_radius=2),
                                    mp_drawing.DrawingSpec(color=(245,66,230), thickness=2, circle_radius=2)
                                    )

            cv2.imshow('Mediapipe Feed', image)

        if cv2.waitKey(10) & 0xFF == ord('q'):
            break

cap.release()
cv2.destroyAllWindows()