582 lines
26 KiB
Python
582 lines
26 KiB
Python
"""
|
|
@file : hpe_classification.py
|
|
@author: yunikim
|
|
@license: DAOOLDNS
|
|
@brief: HPE 자세 추정 알고리즘.
|
|
작업중지동작(cross arm) 및 넘어짐 감지.
|
|
기존의 두 선분의 교차여부, 교차점 좌표 및 해당 좌표의 각 선분 상 위치 비율을 얻는 로직을 통합함.
|
|
|
|
@section Modify History
|
|
- 2023-06-21 yunikim base
|
|
|
|
"""
|
|
|
|
import math
|
|
import numpy as np
|
|
from hpe_classification import config
|
|
import logging
|
|
import project_config
|
|
|
|
|
|
class HPETypeMask:
|
|
NORMAL = 0x0000
|
|
FALL_DOWN = 0x0080
|
|
CROSS_ARM = 0x0100
|
|
|
|
|
|
class HPEClassification:
|
|
# keypoint는 실제 사용번호와 index가 다르다.
|
|
# index 0~16 => keypoint 1~17
|
|
class __KPIndex:
|
|
NOSE = 0 # 코
|
|
RIGHT_EYE = 1 # 눈
|
|
LEFT_EYE = 2
|
|
RIGHT_EAR = 3 # 귀
|
|
LEFT_EAR = 4
|
|
RIGHT_SHOULDER = 5 # 어깨
|
|
LEFT_SHOULDER = 6
|
|
RIGHT_ELBOW = 7 # 팔꿈치
|
|
LEFT_ELBOW = 8
|
|
RIGHT_WRIST = 9 # 손목
|
|
LEFT_WRIST = 10
|
|
RIGHT_HIP = 11 # 엉덩이
|
|
LEFT_HIP = 12
|
|
RIGHT_KNEE = 13 # 무릎
|
|
LEFT_KNEE = 14
|
|
RIGHT_FEET = 15 # 발
|
|
LEFT_FEET = 16
|
|
MAX = 17
|
|
|
|
class __HPELevel:
|
|
NORMAL = 0
|
|
FALLDOWN = 8
|
|
CROSSARM = 9
|
|
|
|
# parameter:
|
|
# pose_info: <dict> {
|
|
# 'person': (cx, cy, w, h, c),
|
|
# 'keypoints': [ (x, y, c), None, (x, y, c), ... ] # 1번이 index 0, 2번 index 1, ... 17번 index 16
|
|
# }
|
|
def __init__(self, pose_info: dict,
|
|
cross_ratio_threshold: float = config.CROSS_ARM_RATIO_THRESHOLD,
|
|
cross_angle_threshold: tuple = config.CROSS_ARM_ANGLE_THRESHOLD,
|
|
arm_angle_threshold: tuple = config.ARM_ANGLE_THRESHOLD,
|
|
elbow_angle_threshold: tuple = config.ELBOW_ANGLE_THRESHOLD,
|
|
falldown_tilt_ratio: float = config.FALLDOWN_TILT_RATIO,
|
|
falldown_tilt_angle: float = config.FALLDOWN_TILT_ANGLE,
|
|
body_tilt_ratio: float = config.BODY_TILT_RATIO,
|
|
yolo_version: int = config.DEFAULT_YOLO_VERSION):
|
|
"""
|
|
:param pose_info: HPE 좌표 정보
|
|
:param cross_ratio_threshold: 팔 교차시 최소 비율
|
|
:param cross_angle_threshold: 팔 교차시 검출 각도 범위. (min, max)
|
|
:param arm_angle_threshold: 팔 각도 범위. (min, max)
|
|
:param elbow_angle_threshold: 팔꿈치 각도 범위. (min, max)
|
|
:param falldown_tilt_ratio: 넘어짐 감지를 위한 비율
|
|
:param falldown_tilt_angle: 작업중 상체 구부러짐 판정각
|
|
:param body_tilt_ratio: 작업중 상체 구부러짐 판정을 위한 비율 임계치
|
|
:param yolo_version: yolo version
|
|
"""
|
|
|
|
self.__raw_data = pose_info
|
|
self.__bounding_box = pose_info['person']
|
|
self.__keypoints = pose_info['keypoints']
|
|
|
|
# logger 설정
|
|
self.__logger = self.__set_logger(self.__class__.__name__)
|
|
|
|
# yolov8 대응
|
|
# v8 에서는 잡히지 않은 point => (0,0)
|
|
# v7 에서는 잡히지 않은 point => None
|
|
if yolo_version == 8:
|
|
self.__convert_zero_point_to_none(self.__keypoints)
|
|
|
|
# 작업중지동작 detect 관련 변수
|
|
self.__cross_ratio_threshold = cross_ratio_threshold
|
|
self.__cross_angle_threshold = cross_angle_threshold
|
|
self.__cross_point = None
|
|
self.__cross_ratio = None
|
|
self.__cross_angle = None
|
|
self.__line1_point1 = self.__keypoints[self.__KPIndex.RIGHT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.RIGHT_ELBOW] else None
|
|
self.__line1_point2 = self.__keypoints[self.__KPIndex.RIGHT_WRIST][0:2] if self.__keypoints[self.__KPIndex.RIGHT_WRIST] else None
|
|
self.__line2_point1 = self.__keypoints[self.__KPIndex.LEFT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.LEFT_ELBOW] else None
|
|
self.__line2_point2 = self.__keypoints[self.__KPIndex.LEFT_WRIST][0:2] if self.__keypoints[self.__KPIndex.LEFT_WRIST] else None
|
|
|
|
self.__detect_cross_arms() # 팔 교차 detect
|
|
|
|
# 팔각도 detect 관련 변수
|
|
self.__cross_arm_angle_threshold = arm_angle_threshold
|
|
self.__cross_left_arm_angle = None
|
|
self.__cross_right_arm_angle = None
|
|
|
|
self.__cross_elbow_angle_threshold = elbow_angle_threshold
|
|
self.__cross_left_elbow_angle = None
|
|
self.__cross_right_elbow_angle = None
|
|
|
|
self.__left_wrist = self.__keypoints[self.__KPIndex.LEFT_WRIST][0:2] if self.__keypoints[self.__KPIndex.LEFT_WRIST] else None
|
|
self.__left_elbow = self.__keypoints[self.__KPIndex.LEFT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.LEFT_ELBOW] else None
|
|
self.__left_shoulder = self.__keypoints[self.__KPIndex.LEFT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.LEFT_SHOULDER] else None
|
|
|
|
self.__right_wrist = self.__keypoints[self.__KPIndex.RIGHT_WRIST][0:2] if self.__keypoints[self.__KPIndex.RIGHT_WRIST] else None
|
|
self.__right_elbow = self.__keypoints[self.__KPIndex.RIGHT_ELBOW][0:2] if self.__keypoints[self.__KPIndex.RIGHT_ELBOW] else None
|
|
self.__right_shoulder = self.__keypoints[self.__KPIndex.RIGHT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.RIGHT_SHOULDER] else None
|
|
|
|
self.__detect_angle_arms() # 팔 각도 detect
|
|
|
|
# 넘어짐 detect 관련 변수
|
|
self.__falldown_tilt_ratio = falldown_tilt_ratio
|
|
self.__falldown_tilt_angle = falldown_tilt_angle
|
|
self.__body_tilt_ratio = body_tilt_ratio
|
|
self.__body_tilt_angle = None
|
|
self.__overturn_upper = False
|
|
self.__overturn_lower = False
|
|
self.__badly_tilt = False
|
|
self.__kp_rhip = self.__keypoints[self.__KPIndex.RIGHT_HIP][0:2] if self.__keypoints[self.__KPIndex.RIGHT_HIP] else None
|
|
self.__kp_lhip = self.__keypoints[self.__KPIndex.LEFT_HIP][0:2] if self.__keypoints[self.__KPIndex.LEFT_HIP] else None
|
|
self.__kp_rsldr = self.__keypoints[self.__KPIndex.RIGHT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.RIGHT_SHOULDER] else None
|
|
self.__kp_lsldr = self.__keypoints[self.__KPIndex.LEFT_SHOULDER][0:2] if self.__keypoints[self.__KPIndex.LEFT_SHOULDER] else None
|
|
self.__kp_rft = self.__keypoints[self.__KPIndex.RIGHT_FEET][0:2] if self.__keypoints[self.__KPIndex.RIGHT_FEET] else None
|
|
self.__kp_lft = self.__keypoints[self.__KPIndex.LEFT_FEET][0:2] if self.__keypoints[self.__KPIndex.LEFT_FEET] else None
|
|
|
|
self.__detect_falldown() # 넘어짐 detect
|
|
|
|
@property
|
|
def falldown_ratio(self):
|
|
return self.__falldown_tilt_ratio
|
|
|
|
@property
|
|
def cross_threshold(self):
|
|
return self.__cross_ratio_threshold
|
|
|
|
@property
|
|
def body_tilt_angle(self):
|
|
return self.__body_tilt_angle
|
|
|
|
def __convert_zero_point_to_none(self, points):
|
|
"""
|
|
좌표가 (0,0)인 경우 None으로 대체
|
|
(yolov8 대응)
|
|
:param points: 좌표리스트
|
|
"""
|
|
for idx, point in enumerate(points):
|
|
if point is not None and point[0] == 0 and point[1] == 0:
|
|
points[idx] = None
|
|
|
|
def get_pose_info(self):
|
|
return self.__raw_data
|
|
|
|
def get_keypoints(self):
|
|
return self.__keypoints
|
|
|
|
def get_bounding_box(self):
|
|
return self.__bounding_box
|
|
|
|
def is_cross_arms(self):
|
|
"""
|
|
팔이 cross 상태인지 여부 확인
|
|
"""
|
|
if self.__cross_point is not None and self.__cross_ratio is not None and self.__cross_angle is not None:
|
|
if self.__cross_ratio[0] > self.__cross_ratio_threshold and self.__cross_ratio[1] > self.__cross_ratio_threshold:
|
|
if self.__cross_angle_threshold[0] < self.__cross_angle < self.__cross_angle_threshold[1]:
|
|
return True
|
|
|
|
return False
|
|
|
|
def is_angle_arms(self):
|
|
"""
|
|
팔이 꺽여있는지 확인
|
|
"""
|
|
if project_config.ADD_CROSS_ARM:
|
|
if self.__cross_left_arm_angle is not None and self.__cross_right_arm_angle is not None:
|
|
if self.__cross_arm_angle_threshold[0] <= self.__cross_left_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_left_arm_angle and \
|
|
self.__cross_arm_angle_threshold[0] <= self.__cross_right_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_right_arm_angle:
|
|
return True
|
|
else:
|
|
if self.__cross_left_arm_angle is not None and self.__cross_right_arm_angle is not None and self.__cross_left_elbow_angle is not None and self.__cross_left_elbow_angle is not None:
|
|
if (self.__cross_arm_angle_threshold[0] <= self.__cross_left_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_left_arm_angle) and \
|
|
(self.__cross_arm_angle_threshold[0] <= self.__cross_right_arm_angle and self.__cross_arm_angle_threshold[1] >= self.__cross_right_arm_angle) and \
|
|
(self.__cross_elbow_angle_threshold[0] <= self.__cross_left_elbow_angle and self.__cross_elbow_angle_threshold[1] >= self.__cross_left_arm_angle) and \
|
|
(self.__cross_elbow_angle_threshold[0] <= self.__cross_right_elbow_angle and self.__cross_elbow_angle_threshold[1] >= self.__cross_right_arm_angle):
|
|
return True
|
|
return False
|
|
|
|
def get_cross_point(self):
|
|
if self.__cross_point is None:
|
|
self.__logger.debug("no cross info")
|
|
return self.__cross_point
|
|
|
|
def get_cross_ratio(self):
|
|
if self.__cross_ratio is None:
|
|
self.__logger.debug("no cross info")
|
|
return self.__cross_ratio
|
|
|
|
def get_cross_angle(self):
|
|
if self.__cross_angle is None:
|
|
self.__logger.debug("no cross info")
|
|
return self.__cross_angle
|
|
|
|
def set_cross_ratio_threshold(self, threshold):
|
|
"""
|
|
교차여부를 결정할 때 사용하는 교차점 위치 최소 비율 설정
|
|
:param threshold: 0.0 ~ 0.5 사이의 값
|
|
"""
|
|
self.__cross_ratio_threshold = threshold
|
|
|
|
def set_cross_angle_threshold(self, angle_range: tuple):
|
|
"""
|
|
교차여부를 결정할 때 사용하는 교차각 범위 설정
|
|
:param angle_range: 교차각 범위, ex) (15.0, 165.0)
|
|
"""
|
|
self.__cross_angle_threshold = angle_range
|
|
|
|
def set_falldown_tilt_ratio(self, tilt_ratio):
|
|
self.__falldown_tilt_ratio = tilt_ratio
|
|
self.__detect_falldown()
|
|
|
|
def set_falldown_tilt_angle(self, tilt_angle):
|
|
"""
|
|
:param tilt_angle: 작업중 상체 구부러짐 판정각
|
|
"""
|
|
self.__falldown_tilt_angle = tilt_angle
|
|
self.__detect_falldown()
|
|
|
|
def is_falldown(self, is_working_on=True):
|
|
"""
|
|
넘어짐 상태 여부 확인
|
|
:param is_working_on: 현재 작업중인지 여부. 작업중인 상태에서는 상체 구부러짐 각도를 반영한다.
|
|
"""
|
|
result = True if self.__overturn_upper or self.__overturn_lower or self.__badly_tilt else False
|
|
tilt_angle = False
|
|
if self.__body_tilt_angle is not None:
|
|
if (self.__body_tilt_angle[0] < self.__falldown_tilt_angle) or (self.__body_tilt_angle[1] < self.__falldown_tilt_angle):
|
|
tilt_angle = True
|
|
|
|
return result or (is_working_on and tilt_angle)
|
|
|
|
def get_hpe_type(self, query=None, is_working_on=True):
|
|
"""
|
|
감지된 Pose의 bit masking 획득하거나 특정 pose 여부를 확인한다.
|
|
:param query: 확인하고자 하는 Pose Mask. 생략시 Pose bit mask 리턴
|
|
:param is_working_on: 현재 작업중인지 여부. 작업중인 상태에서는 상체 구부러짐 각도를 반영한다.
|
|
:return: query 입력시 True/False, 미입력시 Pose bit mask
|
|
"""
|
|
result = 0x0000
|
|
|
|
if self.is_falldown(is_working_on):
|
|
result |= HPETypeMask.FALL_DOWN
|
|
if project_config.ADD_CROSS_ARM:
|
|
if self.is_cross_arms():
|
|
result |= HPETypeMask.CROSS_ARM
|
|
elif self.is_angle_arms():
|
|
result |= HPETypeMask.CROSS_ARM
|
|
else:
|
|
if self.is_angle_arms():
|
|
result |= HPETypeMask.CROSS_ARM
|
|
self.__logger.debug(msg=f"***angle arms -> left angle: {int(self.__cross_left_arm_angle)}, right angle: {int(self.__cross_right_arm_angle)}, left elbow: {int(self.__cross_left_elbow_angle)}, right elbow: {int(self.__cross_right_elbow_angle)}")
|
|
|
|
if query is None:
|
|
return result
|
|
|
|
return (result & query) != 0
|
|
|
|
# 현재 type별 level값을 return 하고 있으나,
|
|
# 추후 특정 계산식으로 level을 계산하게 될 예정
|
|
def get_hpe_level(self, is_working_on=True):
|
|
"""
|
|
:param is_working_on: 현재 작업중인지 여부. 작업중인 상태에서는 상체 구부러짐 각도를 반영한다.
|
|
"""
|
|
result = 0
|
|
|
|
if self.is_falldown(is_working_on) and result < self.__HPELevel.FALLDOWN:
|
|
result = self.__HPELevel.FALLDOWN
|
|
|
|
if (self.is_cross_arms() or self.is_angle_arms()) and result < self.__HPELevel.CROSSARM:
|
|
result = self.__HPELevel.CROSSARM
|
|
|
|
return result
|
|
|
|
def __detect_cross_arms(self):
|
|
self.__cross_point = self.__cross_ratio = self.__cross_angle = None
|
|
|
|
if not self.__line1_point1 or not self.__line1_point2 or not self.__line2_point1 or not self.__line2_point2:
|
|
self.__logger.debug("need 4 points for detecting cross arms")
|
|
return
|
|
|
|
# 손목이 팔꿈치 아래에 있는 경우는 제외
|
|
if self.__line1_point1[1] < self.__line2_point2[1] and self.__line2_point1[1] < self.__line1_point2[1]:
|
|
self.__logger.debug("both wrist is below elbow.")
|
|
return
|
|
|
|
line1_ccw = self.__ccw(self.__line1_point1, self.__line1_point2, self.__line2_point1) * \
|
|
self.__ccw(self.__line1_point1, self.__line1_point2, self.__line2_point2)
|
|
line2_ccw = self.__ccw(self.__line2_point1, self.__line2_point2, self.__line1_point1) * \
|
|
self.__ccw(self.__line2_point1, self.__line2_point2, self.__line1_point2)
|
|
|
|
if line1_ccw < 0 and line2_ccw < 0:
|
|
self.__cross_point = self.__get_cross_point()
|
|
self.__cross_ratio = self.__get_cross_ratio()
|
|
self.__cross_angle = self.__get_cross_angle()
|
|
|
|
# 두 선분의 교차지점 좌표 획득.
|
|
# 교차됨이 확인 된 후에 사용할 것.
|
|
# 출력: 교차지점 좌표 tuple
|
|
def __get_cross_point(self):
|
|
"""
|
|
두 직선의 교차점 좌표를 구함
|
|
:return:
|
|
"""
|
|
x1, y1 = self.__line1_point1[0:2]
|
|
x2, y2 = self.__line1_point2[0:2]
|
|
x3, y3 = self.__line2_point1[0:2]
|
|
x4, y4 = self.__line2_point2[0:2]
|
|
|
|
cx = ((x1 * y2 - x2 * y1) * (x3 - x4) - (x1 - x2) * (x3 * y4 - x4 * y3)) / \
|
|
((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4))
|
|
cy = ((x1 * y2 - x2 * y1) * (y3 - y4) - (y1 - y2) * (x3 * y4 - x4 * y3)) / \
|
|
((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4))
|
|
|
|
return cx, cy
|
|
|
|
def __get_cross_angle(self, p1=None, p2=None, cross_point=None):
|
|
"""
|
|
교차하는 팔의 각도를 구함
|
|
:param p1: p2: 구하고자 하는 vector 방향의 각 좌표
|
|
:param cross_point: 교차점
|
|
"""
|
|
if cross_point is None and self.__cross_point is None:
|
|
return None
|
|
|
|
p1 = self.__line1_point1[0:2] if p1 is None else p1 # 오른 팔꿈치
|
|
p2 = self.__line2_point2[0:2] if p2 is None else p2 # 왼 손목
|
|
cross_point = self.__cross_point if cross_point is None else cross_point
|
|
|
|
return self.three_point_angle(p1, p2, cross_point)
|
|
|
|
@staticmethod
|
|
def three_point_angle(p1, p2, cross_point):
|
|
"""
|
|
세 점 사이의 각도를 구함
|
|
"""
|
|
x1, y1 = p1[0:2]
|
|
x2, y2 = p2[0:2]
|
|
cx, cy = cross_point[0:2]
|
|
|
|
radian = np.arctan2((y1-cy), (x1-cx)) - np.arctan2((y2-cy), (x2-cx))
|
|
radian = np.fabs(radian * 180 / np.pi)
|
|
if radian > 180:
|
|
radian = 360 - radian
|
|
|
|
return radian
|
|
|
|
def __get_cross_ratio(self):
|
|
"""
|
|
선분 위의 점이 해당 선분의 어느정도 위치에 있는지 확인
|
|
(끝점에서 얼마나 떨어져 있는지 선분에 대한 비율)
|
|
:return: 작은 비율 기준으로 리턴됨. 0.0 ~ 0.5, 0.5 이면 가운데
|
|
"""
|
|
length_line1 = math.dist(self.__line1_point1[0:2], self.__line1_point2[0:2])
|
|
length_point1 = math.dist(self.__line1_point1[0:2], self.__cross_point)
|
|
|
|
ratio1 = length_point1 / length_line1
|
|
if ratio1 > 0.5:
|
|
ratio1 = math.fabs(1.0 - ratio1)
|
|
|
|
length_line2 = math.dist(self.__line2_point1[0:2], self.__line2_point2[0:2])
|
|
length_point2 = math.dist(self.__line2_point1[0:2], self.__cross_point)
|
|
|
|
ratio2 = length_point2 / length_line2
|
|
if ratio2 > 0.5:
|
|
ratio2 = math.fabs(1.0 - ratio2)
|
|
|
|
return ratio1, ratio2
|
|
|
|
# 입력: 3개의 point, 각 (x, y)
|
|
# 출력: 0, 1, -1
|
|
@staticmethod
|
|
def __ccw(point1, point2, point3):
|
|
"""
|
|
3개의 점에 대한 CCW 계산 함수
|
|
:return: -1, 0, 1
|
|
"""
|
|
x1, y1 = point1[0:2]
|
|
x2, y2 = point2[0:2]
|
|
x3, y3 = point3[0:2]
|
|
cross_product = ((x2 - x1) * (y3 - y1)) - ((x3 - x1) * (y2 - y1))
|
|
|
|
if cross_product > 0:
|
|
return 1 # 반시계방향 (counter clockwise)
|
|
elif cross_product < 0:
|
|
return -1 # 시계방향 (clockwise)
|
|
|
|
return 0 # 평행 (collinear)
|
|
|
|
def __detect_angle_arms(self):
|
|
self.__cross_left_arm_angle = self.__cross_right_arm_angle = self.__cross_left_elbow_angle = self.__cross_right_elbow_angle = None
|
|
|
|
if project_config.ADD_CROSS_ARM:
|
|
if self.__left_elbow is None or self.__left_shoulder is None or self.__right_elbow is None or self.__right_shoulder is None:
|
|
self.__logger.debug("need 4 points for detecting angle arms")
|
|
return
|
|
else:
|
|
if self.__left_elbow is None or self.__left_shoulder is None or self.__right_elbow is None or self.__right_shoulder is None or \
|
|
self.__left_wrist is None or self.__right_wrist is None:
|
|
self.__logger.debug("need 6 points for detecting angle arms")
|
|
return
|
|
|
|
self.__cross_left_arm_angle, self.__cross_right_arm_angle, self.__cross_left_elbow_angle ,self.__cross_right_elbow_angle = self.__get_angle_arms()
|
|
|
|
@staticmethod
|
|
def three_point_arm_angle(p1, p2, cross_point):
|
|
"""
|
|
세 점 사이(팔)의 각도를 구함
|
|
p1: 몸쪽에서 가장 먼 keypoint
|
|
p2: 몸쪽에서 가장 가까운 keypoint
|
|
"""
|
|
x1, y1 = p1[0:2]
|
|
x2, y2 = p2[0:2]
|
|
cx, cy = cross_point[0:2]
|
|
|
|
radian = np.arctan2((y1-cy), (x1-cx)) - np.arctan2((y2-cy), (x2-cx))
|
|
radian = np.fabs(radian * 180 / np.pi)
|
|
if y1 > cy or y1 > y2:
|
|
return None
|
|
|
|
if radian > 180 :
|
|
radian = 360 - radian
|
|
|
|
return radian
|
|
|
|
def __get_angle_arms(self):
|
|
left_angle = self.three_point_arm_angle(p1=self.__left_elbow,cross_point=self.__left_shoulder,p2=self.__right_shoulder)
|
|
right_angle = self.three_point_arm_angle(p1=self.__right_elbow,cross_point=self.__right_shoulder,p2=self.__left_shoulder)
|
|
left_elbow = None if project_config.ADD_CROSS_ARM else self.three_point_arm_angle(p1=self.__left_wrist,cross_point=self.__left_elbow,p2=self.__left_shoulder)
|
|
right_elbow = None if project_config.ADD_CROSS_ARM else self.three_point_arm_angle(p1=self.__right_wrist,cross_point=self.__right_elbow,p2=self.__right_shoulder)
|
|
|
|
return left_angle,right_angle,left_elbow,right_elbow
|
|
|
|
def __detect_falldown(self):
|
|
"""
|
|
인스턴스 생성시 주어진 key points를 바탕으로 falldown 판단
|
|
- shoulder 좌표가 hip 좌표 아래에 있는 경우: __overturn_upper set
|
|
- hip 좌표가 feet 좌표 아래에 있는 경우: __overturn_lower set
|
|
- shoulder 좌표 및 feet 좌표의 ratio를 계산하여 임계치 이하인 경우: __badly_tilt set
|
|
- shoulder 좌표 및 hip 좌표를 통해 body tilt angle 계산 __body_tilt_angle에 저장
|
|
"""
|
|
self.__overturn_upper = False
|
|
self.__overturn_lower = False
|
|
self.__badly_tilt = False
|
|
self.__body_tilt_angle = None
|
|
|
|
# 1. 상체 뒤집힘
|
|
if self.__kp_lhip and self.__kp_rhip and self.__kp_rsldr and self.__kp_lsldr:
|
|
if self.__kp_lhip[1] < self.__kp_lsldr[1] and self.__kp_rhip[1] < self.__kp_rsldr[1]:
|
|
self.__overturn_upper = True
|
|
else:
|
|
self.__logger.debug("need hip and shoulder points for detecting upper body falldown")
|
|
|
|
# 2. 하체 뒤집힘
|
|
if self.__kp_rft and self.__kp_lft and self.__kp_lhip and self.__kp_rhip:
|
|
if self.__kp_lhip[1] > self.__kp_lft[1] and self.__kp_rhip[1] > self.__kp_rft[1]:
|
|
self.__overturn_lower = True
|
|
else:
|
|
self.__logger.debug("need feet points for detecting lower body falldown")
|
|
|
|
# 3. 신체의 기울어짐 (좌우 어깨, 좌우 발)
|
|
if self.__kp_lft and self.__kp_rft and self.__kp_rsldr and self.__kp_lsldr:
|
|
xmax = np.max([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lft[0], self.__kp_rft[0]])
|
|
ymax = np.max([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lft[1], self.__kp_rft[1]])
|
|
xmin = np.min([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lft[0], self.__kp_rft[0]])
|
|
ymin = np.min([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lft[1], self.__kp_rft[1]])
|
|
body_width = math.fabs(xmax - xmin)
|
|
body_height = math.fabs(ymax - ymin)
|
|
|
|
if (body_width > 1e-4) and ((body_height / body_width) < self.__falldown_tilt_ratio):
|
|
self.__badly_tilt = True
|
|
else:
|
|
self.__logger.debug("need feet and shoulder points for detecting badly tilt")
|
|
|
|
# 4. 상체 기울어짐 (좌우 어깨, 좌우 엉덩이)
|
|
if self.__kp_lhip and self.__kp_rhip and self.__kp_rsldr and self.__kp_lsldr:
|
|
angle_left = self.three_point_angle(self.__kp_lsldr, self.__kp_rhip, self.__kp_lhip)
|
|
angle_right = self.three_point_angle(self.__kp_rsldr, self.__kp_lhip, self.__kp_rhip)
|
|
self.__body_tilt_angle = (angle_left, angle_right)
|
|
|
|
xmax = np.max([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lhip[0], self.__kp_rhip[0]])
|
|
ymax = np.max([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lhip[1], self.__kp_rhip[1]])
|
|
xmin = np.min([self.__kp_lsldr[0], self.__kp_rsldr[0], self.__kp_lhip[0], self.__kp_rhip[0]])
|
|
ymin = np.min([self.__kp_lsldr[1], self.__kp_rsldr[1], self.__kp_lhip[1], self.__kp_rhip[1]])
|
|
body_width = math.fabs(xmax - xmin)
|
|
body_height = math.fabs(ymax - ymin)
|
|
|
|
if (body_height > 1e-4) and ((body_width / body_height) < self.__body_tilt_ratio):
|
|
self.__body_tilt_angle = None
|
|
else:
|
|
self.__logger.debug("need feet and shoulder points for calculating body tilt angle")
|
|
|
|
def __set_logger(self, name):
|
|
logger = logging.getLogger(name)
|
|
logger_handler = logging.StreamHandler()
|
|
logger_handler.setFormatter(logging.Formatter("[%(levelname)s][%(asctime)s][%(module)s::%(funcName)s()] %(message)s"))
|
|
logger.addHandler(logger_handler)
|
|
logger.setLevel(config.DEFAULT_LOG_LEVEL)
|
|
|
|
return logger
|
|
|
|
def log(self, level, message):
|
|
self.__logger.log(level, message)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# pose_info: <dict> {
|
|
# 'person': (cx, cy, w, h, c),
|
|
# 'keypoints': [ (x, y, c), None, (x, y, c), ... ] # 1번이 index 0, 2번 index 1, ... 17번 index 16
|
|
# }
|
|
pose_info1 = {
|
|
'person': (0, 0, 0, 0, 0),
|
|
'keypoints': [
|
|
(0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0),
|
|
(4.64, 3, 0), (-3.2, 2.85, 0), # 오른쪽, 왼쪽 어깨
|
|
(-2.83, 2.2, 0), (5.45, 3.34, 0), # 오른쪽, 왼쪽 팔꿈치
|
|
(6.5, 1.6, 0), (0.98, 1.77, 0), # 오른쪽, 왼쪽 손목
|
|
(3.4, 3.9, 0), (-8.6, 3.2, 0), # 오른쪽, 왼쪽 엉덩이
|
|
(0, 0, 0), (0, 0, 0),
|
|
(0, 20, 0), (0, 20, 0) # 오른쪽, 왼쪽 발
|
|
]
|
|
}
|
|
test1 = HPEClassification(pose_info1, cross_ratio_threshold=0.1)
|
|
print(f'is_cross_arms({test1.cross_threshold}):', test1.is_cross_arms())
|
|
print('angle:', test1.get_cross_angle())
|
|
test1.set_cross_ratio_threshold(0.05)
|
|
print(f'is_cross_arms({test1.cross_threshold}):', test1.is_cross_arms())
|
|
print(test1.get_cross_point())
|
|
print(test1.get_cross_ratio())
|
|
print('angle:', test1.get_cross_angle())
|
|
print('is_falldown:', test1.is_falldown(True))
|
|
print(test1.get_hpe_type(HPETypeMask.CROSS_ARM))
|
|
print(test1.get_hpe_type(HPETypeMask.FALL_DOWN))
|
|
print(hex(test1.get_hpe_type()))
|
|
print('hpe level:', test1.get_hpe_level())
|
|
print('--------------------------')
|
|
print('body tilt angle:', test1.body_tilt_angle)
|
|
|
|
print('--------------------------')
|
|
|
|
pose_info2 = {
|
|
'person': (0, 0, 0, 0, 0),
|
|
'keypoints': [
|
|
(0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0),
|
|
(5, 20, 0), (3, 17, 0), # 오른쪽, 왼쪽 어깨
|
|
(-2.83, 2.2, 0), (0.98, 1.77, 0), # 오른쪽, 왼쪽 팔꿈치
|
|
(6.5, 1.6, 0), (5.45, 3.34, 0), # 오른쪽, 왼쪽 손목
|
|
(0, 30, 0), (0, 30, 0), # 오른쪽, 왼쪽 엉덩이
|
|
(0, 0, 0), (0, 0, 0),
|
|
(37, 31, 0), (40, 42, 0) # 오른쪽, 왼쪽 발
|
|
]
|
|
}
|
|
test2 = HPEClassification(pose_info2, falldown_tilt_ratio=0.5)
|
|
print('is_cross_arms:', test2.is_cross_arms())
|
|
print(f'is_falldown({test2.falldown_ratio}):', test2.is_falldown())
|
|
test2.set_falldown_tilt_ratio(0.7)
|
|
print(f'is_falldown({test2.falldown_ratio}):', test2.is_falldown())
|
|
print('hpe level:', test2.get_hpe_level())
|