226 | 
227 |
230 | Source 231 |
232 | 233 | 234 | ## Code 235 | 236 | To facilitate reusability, the code is splitted in 2 classes: 237 | - **BlazeposeDepthai**, which is responsible of computing the body landmarks. The importation of this class depends on the mode: 238 | ``` 239 | # For Host mode: 240 | from BlazeposeDepthai import BlazeposeDepthai 241 | ``` 242 | ``` 243 | # For Edge mode: 244 | from BlazeposeDepthaiEdge import BlazeposeDepthai 245 | ``` 246 | - **BlazeposeRenderer**, which is responsible of rendering the landmarks and the skeleton on the video frame. 247 | 248 | This way, you can replace the renderer from this repository and write and personalize your own renderer (for some projects, you may not even need a renderer). 249 | 250 | The file ```demo.py``` is a representative example of how to use these classes: 251 | ``` 252 | from BlazeposeDepthaiEdge import BlazeposeDepthai 253 | from BlazeposeRenderer import BlazeposeRenderer 254 | 255 | # The argparse stuff has been removed to keep only the important code 256 | 257 | tracker = BlazeposeDepthai(input_src=args.input, 258 | pd_model=args.pd_m, 259 | lm_model=args.lm_m, 260 | smoothing=not args.no_smoothing, 261 | xyz=args.xyz, 262 | crop=args.crop, 263 | internal_fps=args.internal_fps, 264 | internal_frame_height=args.internal_frame_height, 265 | force_detection=args.force_detection, 266 | stats=args.stats, 267 | trace=args.trace) 268 | 269 | renderer = BlazeposeRenderer( 270 | pose, 271 | show_3d=args.show_3d, 272 | output=args.output) 273 | 274 | while True: 275 | # Run blazepose on next frame 276 | frame, body = tracker.next_frame() 277 | if frame is None: break 278 | # Draw 2d skeleton 279 | frame = renderer.draw(frame, body) 280 | key = renderer.waitKey(delay=1) 281 | if key == 27 or key == ord('q'): 282 | break 283 | renderer.exit() 284 | tracker.exit() 285 | ``` 286 | 287 | For more information on: 288 | - the arguments of the tracker, please refer to the docstring of class `BlazeposeDepthai` or `BlazeposeDepthaiEdge` in `BlazeposeDepthai.py` or `BlazeposeDepthaiEdge.py`; 289 | - the attributes of the 'body' element you can exploit in your program, please refer to the doctring of class `Body` in `mediapipe_utils.py`. 290 | 291 | ## Examples 292 | 293 | ||| 294 | |-|-| 295 | |[Semaphore alphabet](examples/semaphore_alphabet) |
|
296 |
297 | ## Credits
298 | * [Google Mediapipe](https://github.com/google/mediapipe)
299 | * Katsuya Hyodo a.k.a [Pinto](https://github.com/PINTO0309), the Wizard of Model Conversion !
300 | * [Tai Chi Step by Step For Beginners Training Session 4](https://www.youtube.com/watch?v=oawZ_7wNWrU&ab_channel=MasterSongKungFu)
301 | * [Semaphore with The RCR Museum](https://www.youtube.com/watch?v=DezaTjQYPh0&ab_channel=TheRoyalCanadianRegimentMuseum)
302 |
--------------------------------------------------------------------------------
/custom_models/DetectionBestCandidate.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | import sys, os
4 | sys.path.insert(1, os.path.realpath(os.path.pardir))
5 | from mediapipe_utils import generate_blazepose_anchors
6 | import numpy as np
7 | from math import pi
8 |
9 | anchors = generate_blazepose_anchors()
10 | print(f"Nb anchors: {len(anchors)}")
11 |
12 | detection_input_length = 224
13 |
14 | class DetectionBestCandidate(nn.Module):
15 | def __init__(self):
16 | super(DetectionBestCandidate, self).__init__()
17 | # anchors shape is nb_anchorsx4 [x_center, y_center, width, height]
18 | # Here: width and height is always 1, so we keep just [x_center, y_center]
19 | self.anchors = torch.from_numpy(anchors[:,:2])
20 | self.plus_anchor_center = np.array([[1,0,0,0,1,0,1,0,1,0,1,0], [0,1,0,0,0,1,0,1,0,1,0,1]], dtype=np.float)
21 | self.plus_anchor_center = torch.from_numpy(self.plus_anchor_center)
22 |
23 | def forward(self, x, y):
24 | # x.shape: 1xnb_anchorsx1
25 | # y.shape: 1xnb_anchorsx12
26 |
27 | # decode_bboxes
28 | best_id = torch.argmax(x)
29 | score = x[0][best_id]
30 | score = torch.sigmoid(score)
31 | bbox = y[0,best_id]
32 | bbox = bbox/detection_input_length + torch.mm(self.anchors[best_id].unsqueeze(0), self.plus_anchor_center)[0]
33 |
34 | # bbox : the 4 first elements describe the square bounding box around the head (not used for determining rotated rectangle around the body)
35 | # The 8 following corresponds to 4 (x,y) normalized keypoints coordinates (useful for determining rotated rectangle)
36 | # For full body, keypoints 1 and 2 are used.
37 | # For upper body, keypoints 3 and 4 are used.
38 | # Here we are only interested in full body
39 |
40 | # detections_to_rect (1st part)
41 | sqn_rr_center_xy = bbox[4:6]
42 | sqn_scale_xy = bbox[6:8]
43 |
44 | return torch.cat((score, sqn_rr_center_xy, sqn_scale_xy))
45 |
46 | def test():
47 |
48 | model = DetectionBestCandidate()
49 | X = torch.randn(1, len(anchors), 1, dtype=torch.float)
50 | Y = torch.randn(1, len(anchors), 12, dtype=torch.float)
51 | result = model(X, Y)
52 | print(result)
53 |
54 | def export_onnx():
55 | """
56 | Exports the model to an ONNX file.
57 | """
58 | model = DetectionBestCandidate()
59 | X = torch.randn(1, len(anchors), 1, dtype=torch.float)
60 | Y = torch.randn(1, len(anchors), 12, dtype=torch.float)
61 | onnx_name = "DetectionBestCandidate.onnx"
62 |
63 | print(f"Generating {onnx_name}")
64 | torch.onnx.export(
65 | model,
66 | (X, Y),
67 | onnx_name,
68 | opset_version=10,
69 | do_constant_folding=True,
70 | # verbose=True,
71 | input_names=['Identity_1', 'Identity'],
72 | output_names=['result']
73 | )
74 |
75 | if __name__ == "__main__":
76 |
77 | test()
78 | export_onnx()
--------------------------------------------------------------------------------
/custom_models/DetectionBestCandidate_sh1.blob:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/custom_models/DetectionBestCandidate_sh1.blob
--------------------------------------------------------------------------------
/custom_models/DivideBy255.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | import sys, os
4 | sys.path.insert(1, os.path.realpath(os.path.pardir))
5 | import numpy as np
6 | from math import pi
7 |
8 |
9 |
10 | detection_input_length = 224
11 |
12 | class DivideBy255(nn.Module):
13 | def __init__(self):
14 | super(DivideBy255, self).__init__()
15 |
16 | def forward(self, x):
17 | return x / 255.0
18 |
19 | def test():
20 |
21 | model = DivideBy255()
22 |
23 | X = torch.ones(1, 3, 256, 256, dtype=torch.float)
24 | result = model(X)
25 | print(result)
26 |
27 | def export_onnx():
28 | """
29 | Exports the model to an ONNX file.
30 | """
31 | model = DivideBy255()
32 | X = torch.randn(1, 3, 256, 256, dtype=torch.float)
33 | onnx_name = "DivideBy255.onnx"
34 |
35 | print(f"Generating {onnx_name}")
36 | torch.onnx.export(
37 | model,
38 | (X),
39 | onnx_name,
40 | opset_version=10,
41 | do_constant_folding=True,
42 | # verbose=True,
43 | # input_names=['Identity_1', 'Identity'],
44 | output_names=['input_1']
45 | )
46 |
47 | if __name__ == "__main__":
48 |
49 | test()
50 | export_onnx()
--------------------------------------------------------------------------------
/custom_models/DivideBy255_sh1.blob:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/custom_models/DivideBy255_sh1.blob
--------------------------------------------------------------------------------
/custom_models/README.md:
--------------------------------------------------------------------------------
1 | ## Generation of DetectionBestCandidate.blob
2 | 1) pytorch is needed
3 | 2) Generate the ONNX model *DetectionBestCandidate.onnx* :
4 | ```
5 | python DetectionBestCandidate.py
6 | ```
7 |
8 | 3) Start the tflite2tensorflow docker container:
9 | ```
10 | ../docker_tflite2tensorflow.sh
11 | ```
12 | 4) From the container shell:
13 | ```
14 | cd workdir
15 | ./convert_model.sh -m DetectionBestCandidate # Use `-n #` if you want to change the number of shaves
16 | ```
17 |
18 | ## Generation of DivideBy255.blob
19 | Same as above with 'DivideBy255' instead of 'DetectionBestCandidate'.
20 |
21 |
--------------------------------------------------------------------------------
/custom_models/convert_model.sh:
--------------------------------------------------------------------------------
1 | # This script has to be run from the docker container started by ./docker_tflite2tensorflow.sh
2 |
3 | usage ()
4 | {
5 | echo "Generate a blob from an ONNX model with a specified number of shaves and cmx (nb cmx = nb shaves)"
6 | echo
7 | echo "Usage: ${0} [-m model] [-n nb_shaves]"
8 | echo
9 | echo "model: DetectionBestCandidate or DivideBy255 or other"
10 | echo "nb_shaves must be between 1 and 13 (default=1)"
11 | }
12 |
13 | while getopts ":hm:n:" opt; do
14 | case ${opt} in
15 | h )
16 | usage
17 | exit 0
18 | ;;
19 | m )
20 | model=$OPTARG
21 | ;;
22 | n )
23 | nb_shaves=$OPTARG
24 | ;;
25 | : )
26 | echo "Error: -$OPTARG requires an argument."
27 | usage
28 | exit 1
29 | ;;
30 | \? )
31 | echo "Invalid option: -$OPTARG"
32 | usage
33 | exit 1
34 | ;;
35 | esac
36 | done
37 |
38 | if [ -z "$model" ]
39 | then
40 | usage
41 | exit 1
42 | fi
43 | if [ ! -f $model_onnx ]
44 | then
45 | echo "The model ${model_onnx} does not exist"
46 | echo "Have you run 'python DetectionBestCandidate.py' ?"
47 | exit 1
48 | fi
49 |
50 | if [ -z "$nb_shaves" ]
51 | then
52 | nb_shaves=1
53 | fi
54 | if [ $nb_shaves -lt 1 -o $nb_shaves -gt 13 ]
55 | then
56 | echo "Invalid number of shaves !"
57 | usage
58 | exit 1
59 | fi
60 |
61 | if [ $model = DivideBy255 ]
62 | then
63 | precision=U8
64 | else
65 | precision=FP16
66 | fi
67 |
68 |
69 | model_onnx="${model}.onnx"
70 | model_xml="${model}.xml"
71 | model_blob="${model}_sh${nb_shaves}.blob"
72 |
73 | echo Model: $model_xml $model_blob
74 | echo Shaves $nb_shaves
75 |
76 | ARG=$1
77 | source /opt/intel/openvino_2021/bin/setupvars.sh
78 |
79 | # Use FP16 and make the batch_size explicit.
80 | python /opt/intel/openvino_2021/deployment_tools/model_optimizer/mo_onnx.py \
81 | --input_model $model_onnx --data_type half
82 | /opt/intel/openvino_2021/deployment_tools/tools/compile_tool/compile_tool -d MYRIAD \
83 | -m $model_xml \
84 | -ip $precision \
85 | -VPU_NUMBER_OF_SHAVES $nb_shaves \
86 | -VPU_NUMBER_OF_CMX_SLICES $nb_shaves \
87 | -o $model_blob
88 |
--------------------------------------------------------------------------------
/demo.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 |
3 | from BlazeposeRenderer import BlazeposeRenderer
4 | import argparse
5 |
6 | parser = argparse.ArgumentParser()
7 | parser.add_argument('-e', '--edge', action="store_true",
8 | help="Use Edge mode (postprocessing runs on the device)")
9 | parser_tracker = parser.add_argument_group("Tracker arguments")
10 | parser_tracker.add_argument('-i', '--input', type=str, default="rgb",
11 | help="'rgb' or 'rgb_laconic' or path to video/image file to use as input (default=%(default)s)")
12 | parser_tracker.add_argument("--pd_m", type=str,
13 | help="Path to an .blob file for pose detection model")
14 | parser_tracker.add_argument("--lm_m", type=str,
15 | help="Landmark model ('full' or 'lite' or 'heavy') or path to an .blob file")
16 | parser_tracker.add_argument('-xyz', '--xyz', action="store_true",
17 | help="Get (x,y,z) coords of reference body keypoint in camera coord system (only for compatible devices)")
18 | parser_tracker.add_argument('-c', '--crop', action="store_true",
19 | help="Center crop frames to a square shape before feeding pose detection model")
20 | parser_tracker.add_argument('--no_smoothing', action="store_true",
21 | help="Disable smoothing filter")
22 | parser_tracker.add_argument('-f', '--internal_fps', type=int,
23 | help="Fps of internal color camera. Too high value lower NN fps (default= depends on the model)")
24 | parser_tracker.add_argument('--internal_frame_height', type=int, default=640,
25 | help="Internal color camera frame height in pixels (default=%(default)i)")
26 | parser_tracker.add_argument('-s', '--stats', action="store_true",
27 | help="Print some statistics at exit")
28 | parser_tracker.add_argument('-t', '--trace', action="store_true",
29 | help="Print some debug messages")
30 | parser_tracker.add_argument('--force_detection', action="store_true",
31 | help="Force person detection on every frame (never use landmarks from previous frame to determine ROI)")
32 |
33 | parser_renderer = parser.add_argument_group("Renderer arguments")
34 | parser_renderer.add_argument('-3', '--show_3d', choices=[None, "image", "world", "mixed"], default=None,
35 | help="Display skeleton in 3d in a separate window. See README for description.")
36 | parser_renderer.add_argument("-o","--output",
37 | help="Path to output video file")
38 |
39 |
40 | args = parser.parse_args()
41 |
42 | if args.edge:
43 | from BlazeposeDepthaiEdge import BlazeposeDepthai
44 | else:
45 | from BlazeposeDepthai import BlazeposeDepthai
46 | tracker = BlazeposeDepthai(input_src=args.input,
47 | pd_model=args.pd_m,
48 | lm_model=args.lm_m,
49 | smoothing=not args.no_smoothing,
50 | xyz=args.xyz,
51 | crop=args.crop,
52 | internal_fps=args.internal_fps,
53 | internal_frame_height=args.internal_frame_height,
54 | force_detection=args.force_detection,
55 | stats=True,
56 | trace=args.trace)
57 |
58 | renderer = BlazeposeRenderer(
59 | tracker,
60 | show_3d=args.show_3d,
61 | output=args.output)
62 |
63 | while True:
64 | # Run blazepose on next frame
65 | frame, body = tracker.next_frame()
66 | if frame is None: break
67 | # Draw 2d skeleton
68 | frame = renderer.draw(frame, body)
69 | key = renderer.waitKey(delay=1)
70 | if key == 27 or key == ord('q'):
71 | break
72 | renderer.exit()
73 | tracker.exit()
--------------------------------------------------------------------------------
/docker_tflite2tensorflow.sh:
--------------------------------------------------------------------------------
1 | # Used to convert original Mediapipe tflite models to Openvino IR and MyriadX blob
2 | # https://github.com/PINTO0309/tflite2tensorflow
3 | #
4 | # First run: docker pull pinto0309/tflite2tensorflow
5 | docker run -it --rm \
6 | -v `pwd`:/home/user/workdir \
7 | -v /tmp/.X11-unix/:/tmp/.X11-unix:rw \
8 | --device /dev/video0:/dev/video0:mwr \
9 | --net=host \
10 | -e XDG_RUNTIME_DIR=$XDG_RUNTIME_DIR \
11 | -e DISPLAY=$DISPLAY \
12 | --privileged \
13 | pinto0309/tflite2tensorflow:latest bash
14 |
15 |
--------------------------------------------------------------------------------
/examples/semaphore_alphabet/README.md:
--------------------------------------------------------------------------------
1 | # Semaphore alphabet with Blazepose on DepthAI
2 |
3 | This demo demonstrates the recognition of semaphore alphabet.
4 |
5 | For each arm (segment shoulder-elbow), its angle with the vertical axe is calculated. The pair of left and right arm angles gives the letter.
6 |
7 |
8 | 
9 |
10 | ## Usage
11 |
12 | ```
13 | -> python3 demo.py -h
14 | usage: demo.py [-h] [-m {full,lite,831}] [-i INPUT] [-o OUTPUT]
15 |
16 | optional arguments:
17 | -h, --help show this help message and exit
18 | -m {full,lite,831}, --model {full,lite,831}
19 | Landmark model to use (default=full
20 | -i INPUT, --input INPUT
21 | 'rgb' or 'rgb_laconic' or path to video/image file to
22 | use as input (default: rgb)
23 | -o OUTPUT, --output OUTPUT
24 | Path to output video file
25 | ```
26 |
27 | ## Credits
28 |
29 | * [Semaphore with The RCR Museum](https://www.youtube.com/watch?v=DezaTjQYPh0&ab_channel=TheRoyalCanadianRegimentMuseum)
--------------------------------------------------------------------------------
/examples/semaphore_alphabet/demo.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | from math import atan2, degrees
3 | import sys
4 | sys.path.append("../..")
5 | from BlazeposeDepthai import BlazeposeDepthai
6 | from BlazeposeRenderer import BlazeposeRenderer
7 | from mediapipe_utils import KEYPOINT_DICT
8 | import argparse
9 |
10 |
11 | # For gesture demo
12 | semaphore_flag = {
13 | (3,4):'A', (2,4):'B', (1,4):'C', (0,4):'D',
14 | (4,7):'E', (4,6):'F', (4,5):'G', (2,3):'H',
15 | (0,3):'I', (0,6):'J', (3,0):'K', (3,7):'L',
16 | (3,6):'M', (3,5):'N', (2,1):'O', (2,0):'P',
17 | (2,7):'Q', (2,6):'R', (2,5):'S', (1,0):'T',
18 | (1,7):'U', (0,5):'V', (7,6):'W', (7,5):'X',
19 | (1,6):'Y', (5,6):'Z',
20 | }
21 |
22 | def recognize_gesture(b):
23 | # b: body
24 |
25 | def angle_with_y(v):
26 | # v: 2d vector (x,y)
27 | # Returns angle in degree of v with y-axis of image plane
28 | if v[1] == 0:
29 | return 90
30 | angle = atan2(v[0], v[1])
31 | return degrees(angle)
32 |
33 | # For the demo, we want to recognize the flag semaphore alphabet
34 | # For this task, we just need to measure the angles of both arms with vertical
35 | right_arm_angle = angle_with_y(b.landmarks[KEYPOINT_DICT['right_elbow'],:2] - b.landmarks[KEYPOINT_DICT['right_shoulder'],:2])
36 | left_arm_angle = angle_with_y(b.landmarks[KEYPOINT_DICT['left_elbow'],:2] - b.landmarks[KEYPOINT_DICT['left_shoulder'],:2])
37 | right_pose = int((right_arm_angle +202.5) / 45) % 8
38 | left_pose = int((left_arm_angle +202.5) / 45) % 8
39 | letter = semaphore_flag.get((right_pose, left_pose), None)
40 | return letter
41 |
42 | parser = argparse.ArgumentParser()
43 | parser.add_argument("-m", "--model", type=str, choices=['full', 'lite', '831'], default='full',
44 | help="Landmark model to use (default=%(default)s")
45 | parser.add_argument('-i', '--input', type=str, default='rgb',
46 | help="'rgb' or 'rgb_laconic' or path to video/image file to use as input (default: %(default)s)")
47 | parser.add_argument("-o","--output",
48 | help="Path to output video file")
49 | args = parser.parse_args()
50 |
51 | pose = BlazeposeDepthai(input_src=args.input, lm_model=args.model)
52 | renderer = BlazeposeRenderer(pose, output=args.output)
53 |
54 | while True:
55 | # Run blazepose on next frame
56 | frame, body = pose.next_frame()
57 | if frame is None: break
58 | # Draw 2d skeleton
59 | frame = renderer.draw(frame, body)
60 | # Gesture recognition
61 | if body:
62 | letter = recognize_gesture(body)
63 | if letter:
64 | cv2.putText(frame, letter, (frame.shape[1] // 2, 100), cv2.FONT_HERSHEY_PLAIN, 5, (0,190,255), 3)
65 | key = renderer.waitKey(delay=1)
66 | if key == 27 or key == ord('q'):
67 | break
68 | renderer.exit()
69 | pose.exit()
70 |
71 |
--------------------------------------------------------------------------------
/examples/semaphore_alphabet/medias/semaphore.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/examples/semaphore_alphabet/medias/semaphore.gif
--------------------------------------------------------------------------------
/examples/semaphore_alphabet/medias/semaphore.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/examples/semaphore_alphabet/medias/semaphore.mp4
--------------------------------------------------------------------------------
/examples/semaphore_alphabet/medias/video.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/examples/semaphore_alphabet/medias/video.mp4
--------------------------------------------------------------------------------
/img/3d_visualizations.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/img/3d_visualizations.gif
--------------------------------------------------------------------------------
/img/3d_world_visualization.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/img/3d_world_visualization.gif
--------------------------------------------------------------------------------
/img/full_body_landmarks.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/img/full_body_landmarks.png
--------------------------------------------------------------------------------
/img/pipeline_edge_mode.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/img/pipeline_edge_mode.png
--------------------------------------------------------------------------------
/img/pipeline_host_mode.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/img/pipeline_host_mode.png
--------------------------------------------------------------------------------
/img/taichi.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/img/taichi.gif
--------------------------------------------------------------------------------
/mediapipe_utils.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | import numpy as np
3 | from collections import namedtuple
4 | from math import ceil, sqrt, pi, floor, sin, cos, atan2, gcd
5 | from collections import namedtuple
6 |
7 | # To not display: RuntimeWarning: overflow encountered in exp
8 | # in line: scores = 1 / (1 + np.exp(-scores))
9 | np.seterr(over='ignore')
10 |
11 | # Dictionary that maps from joint names to keypoint indices.
12 | KEYPOINT_DICT = {
13 | "nose": 0,
14 | "left_eye_inner": 1,
15 | "left_eye": 2,
16 | "left_eye_outer": 3,
17 | "right_eye_inner": 4,
18 | "right_eye": 5,
19 | "right_eye_outer": 6,
20 | "left_ear": 7,
21 | "right_ear": 8,
22 | "mouth_left": 9,
23 | "mouth_right": 10,
24 | "left_shoulder": 11,
25 | "right_shoulder": 12,
26 | "left_elbow": 13,
27 | "right_elbow": 14,
28 | "left_wrist": 15,
29 | "right_wrist": 16,
30 | "left_pinky": 17,
31 | "right_pinky": 18,
32 | "left_index": 19,
33 | "right_index": 20,
34 | "left_thumb": 21,
35 | "right_thumb": 22,
36 | "left_hip": 23,
37 | "right_hip": 24,
38 | "left_knee": 25,
39 | "right_knee": 26,
40 | "left_ankle": 27,
41 | "right_ankle": 28,
42 | "left_heel": 29,
43 | "right_heel": 30,
44 | "left_foot_index": 31,
45 | "right_foot_index": 32
46 | }
47 |
48 | class Body:
49 | def __init__(self, pd_score=None, pd_box=None, pd_kps=None):
50 | """
51 | Attributes:
52 | pd_score : detection score
53 | pd_box : detection box [x, y, w, h], normalized [0,1] in the squared image
54 | pd_kps : detection keypoints coordinates [x, y], normalized [0,1] in the squared image
55 | rect_x_center, rect_y_center : center coordinates of the rotated bounding rectangle, normalized [0,1] in the squared image
56 | rect_w, rect_h : width and height of the rotated bounding rectangle, normalized in the squared image (may be > 1)
57 | rotation : rotation angle of rotated bounding rectangle with y-axis in radian
58 | rect_x_center_a, rect_y_center_a : center coordinates of the rotated bounding rectangle, in pixels in the squared image
59 | rect_w, rect_h : width and height of the rotated bounding rectangle, in pixels in the squared image
60 | rect_points : list of the 4 points coordinates of the rotated bounding rectangle, in pixels
61 | expressed in the squared image during processing,
62 | expressed in the original rectangular image when returned to the user
63 | lm_score: global landmark score
64 | norm_landmarks : 3D landmarks coordinates in the rotated bounding rectangle, normalized [0,1]
65 | landmarks : 3D landmarks coordinates in the rotated bounding rectangle, in pixel in the original rectangular image
66 | world_landmarks : 3D landmarks coordinates in meter with mid hips point being the origin.
67 | The y value of landmarks_world coordinates is negative for landmarks
68 | above the mid hips (like shoulders) and negative for landmarks below (like feet)
69 | xyz: (optionally) 3D location in camera coordinate system of reference point (mid hips or mid shoulders)
70 | xyz_ref: (optionally) name of the reference point ("mid_hips" or "mid_shoulders"),
71 | xyz_zone: (optionally) 4 int array of zone (in the source image) on which is measured depth.
72 | xyz_zone[0:2] is top-left zone corner in pixels, xyz_zone[2:4] is bottom-right zone corner
73 | """
74 | self.pd_score = pd_score
75 | self.pd_box = pd_box
76 | self.pd_kps = pd_kps
77 | def print(self):
78 | attrs = vars(self)
79 | print('\n'.join("%s: %s" % item for item in attrs.items()))
80 |
81 |
82 | SSDAnchorOptions = namedtuple('SSDAnchorOptions',[
83 | 'num_layers',
84 | 'min_scale',
85 | 'max_scale',
86 | 'input_size_height',
87 | 'input_size_width',
88 | 'anchor_offset_x',
89 | 'anchor_offset_y',
90 | 'strides',
91 | 'aspect_ratios',
92 | 'reduce_boxes_in_lowest_layer',
93 | 'interpolated_scale_aspect_ratio',
94 | 'fixed_anchor_size'])
95 |
96 | def calculate_scale(min_scale, max_scale, stride_index, num_strides):
97 | if num_strides == 1:
98 | return (min_scale + max_scale) / 2
99 | else:
100 | return min_scale + (max_scale - min_scale) * stride_index / (num_strides - 1)
101 |
102 | def generate_anchors(options):
103 | """
104 | option : SSDAnchorOptions
105 | # https://github.com/google/mediapipe/blob/master/mediapipe/calculators/tflite/ssd_anchors_calculator.cc
106 | """
107 | anchors = []
108 | layer_id = 0
109 | n_strides = len(options.strides)
110 | while layer_id < n_strides:
111 | anchor_height = []
112 | anchor_width = []
113 | aspect_ratios = []
114 | scales = []
115 | # For same strides, we merge the anchors in the same order.
116 | last_same_stride_layer = layer_id
117 | while last_same_stride_layer < n_strides and \
118 | options.strides[last_same_stride_layer] == options.strides[layer_id]:
119 | scale = calculate_scale(options.min_scale, options.max_scale, last_same_stride_layer, n_strides)
120 | if last_same_stride_layer == 0 and options.reduce_boxes_in_lowest_layer:
121 | # For first layer, it can be specified to use predefined anchors.
122 | aspect_ratios += [1.0, 2.0, 0.5]
123 | scales += [0.1, scale, scale]
124 | else:
125 | aspect_ratios += options.aspect_ratios
126 | scales += [scale] * len(options.aspect_ratios)
127 | if options.interpolated_scale_aspect_ratio > 0:
128 | if last_same_stride_layer == n_strides -1:
129 | scale_next = 1.0
130 | else:
131 | scale_next = calculate_scale(options.min_scale, options.max_scale, last_same_stride_layer+1, n_strides)
132 | scales.append(sqrt(scale * scale_next))
133 | aspect_ratios.append(options.interpolated_scale_aspect_ratio)
134 | last_same_stride_layer += 1
135 |
136 | for i,r in enumerate(aspect_ratios):
137 | ratio_sqrts = sqrt(r)
138 | anchor_height.append(scales[i] / ratio_sqrts)
139 | anchor_width.append(scales[i] * ratio_sqrts)
140 |
141 | stride = options.strides[layer_id]
142 | feature_map_height = ceil(options.input_size_height / stride)
143 | feature_map_width = ceil(options.input_size_width / stride)
144 |
145 | for y in range(feature_map_height):
146 | for x in range(feature_map_width):
147 | for anchor_id in range(len(anchor_height)):
148 | x_center = (x + options.anchor_offset_x) / feature_map_width
149 | y_center = (y + options.anchor_offset_y) / feature_map_height
150 | # new_anchor = Anchor(x_center=x_center, y_center=y_center)
151 | if options.fixed_anchor_size:
152 | new_anchor = [x_center, y_center, 1.0, 1.0]
153 | # new_anchor.w = 1.0
154 | # new_anchor.h = 1.0
155 | else:
156 | new_anchor = [x_center, y_center, anchor_width[anchor_id], anchor_height[anchor_id]]
157 | # new_anchor.w = anchor_width[anchor_id]
158 | # new_anchor.h = anchor_height[anchor_id]
159 | anchors.append(new_anchor)
160 |
161 | layer_id = last_same_stride_layer
162 | return np.array(anchors)
163 |
164 | def generate_blazepose_anchors():
165 | # https://github.com/google/mediapipe/blob/master/mediapipe/modules/pose_detection/pose_detection_cpu.pbtxt
166 | anchor_options = SSDAnchorOptions(
167 | num_layers=5,
168 | min_scale=0.1484375,
169 | max_scale=0.75,
170 | input_size_height=224,
171 | input_size_width=224,
172 | anchor_offset_x=0.5,
173 | anchor_offset_y=0.5,
174 | strides=[8, 16, 32, 32, 32],
175 | aspect_ratios= [1.0],
176 | reduce_boxes_in_lowest_layer=False,
177 | interpolated_scale_aspect_ratio=1.0,
178 | fixed_anchor_size=True)
179 | return generate_anchors(anchor_options)
180 |
181 | def decode_bboxes(score_thresh, scores, bboxes, anchors, best_only=False):
182 | """
183 | wi, hi : NN input shape
184 | https://github.com/google/mediapipe/blob/master/mediapipe/modules/pose_detection/pose_detection_cpu.pbtxt
185 | # Decodes the detection tensors generated by the TensorFlow Lite model, based on
186 | # the SSD anchors and the specification in the options, into a vector of
187 | # detections. Each detection describes a detected object.
188 | Version 0.8.3.1:
189 | node {
190 | calculator: "TensorsToDetectionsCalculator"
191 | input_stream: "TENSORS:detection_tensors"
192 | input_side_packet: "ANCHORS:anchors"
193 | output_stream: "DETECTIONS:unfiltered_detections"
194 | options: {
195 | [mediapipe.TensorsToDetectionsCalculatorOptions.ext] {
196 | num_classes: 1
197 | num_boxes: 896
198 | num_coords: 12
199 | box_coord_offset: 0
200 | keypoint_coord_offset: 4
201 | num_keypoints: 4
202 | num_values_per_keypoint: 2
203 | sigmoid_score: true
204 | score_clipping_thresh: 100.0
205 | reverse_output_order: true
206 | x_scale: 128.0
207 | y_scale: 128.0
208 | h_scale: 128.0
209 | w_scale: 128.0
210 | min_score_thresh: 0.5
211 | }
212 | }
213 |
214 | Version 0.8.4:
215 | [mediapipe.TensorsToDetectionsCalculatorOptions.ext] {
216 | num_classes: 1
217 | num_boxes: 2254
218 | num_coords: 12
219 | box_coord_offset: 0
220 | keypoint_coord_offset: 4
221 | num_keypoints: 4
222 | num_values_per_keypoint: 2
223 | sigmoid_score: true
224 | score_clipping_thresh: 100.0
225 | reverse_output_order: true
226 | x_scale: 224.0
227 | y_scale: 224.0
228 | h_scale: 224.0
229 | w_scale: 224.0
230 | min_score_thresh: 0.5
231 | }
232 |
233 | # Bounding box in each pose detection is currently set to the bounding box of
234 | # the detected face. However, 4 additional key points are available in each
235 | # detection, which are used to further calculate a (rotated) bounding box that
236 | # encloses the body region of interest. Among the 4 key points, the first two
237 | # are for identifying the full-body region, and the second two for upper body
238 | # only:
239 | #
240 | # Key point 0 - mid hip center
241 | # Key point 1 - point that encodes size & rotation (for full body)
242 | # Key point 2 - mid shoulder center
243 | # Key point 3 - point that encodes size & rotation (for upper body)
244 | #
245 |
246 | scores: shape = [number of anchors 896]
247 | bboxes: shape = [ number of anchors x 12], 12 = 4 (bounding box : (cx,cy,w,h) + 8 (4 palm keypoints)
248 | """
249 | bodies = []
250 | scores = 1 / (1 + np.exp(-scores))
251 | if best_only:
252 | best_id = np.argmax(scores)
253 | if scores[best_id] < score_thresh: return bodies
254 | det_scores = scores[best_id:best_id+1]
255 | det_bboxes = bboxes[best_id:best_id+1]
256 | det_anchors = anchors[best_id:best_id+1]
257 | else:
258 | detection_mask = scores > score_thresh
259 | det_scores = scores[detection_mask]
260 | if det_scores.size == 0: return bodies
261 | det_bboxes = bboxes[detection_mask]
262 | det_anchors = anchors[detection_mask]
263 |
264 | scale = 224 # x_scale, y_scale, w_scale, h_scale
265 |
266 | # cx, cy, w, h = bboxes[i,:4]
267 | # cx = cx * anchor.w / wi + anchor.x_center
268 | # cy = cy * anchor.h / hi + anchor.y_center
269 | # lx = lx * anchor.w / wi + anchor.x_center
270 | # ly = ly * anchor.h / hi + anchor.y_center
271 | det_bboxes = det_bboxes* np.tile(det_anchors[:,2:4], 6) / scale + np.tile(det_anchors[:,0:2],6)
272 | # w = w * anchor.w / wi (in the prvious line, we add anchor.x_center and anchor.y_center to w and h, we need to substract them now)
273 | # h = h * anchor.h / hi
274 | det_bboxes[:,2:4] = det_bboxes[:,2:4] - det_anchors[:,0:2]
275 | # box = [cx - w*0.5, cy - h*0.5, w, h]
276 | det_bboxes[:,0:2] = det_bboxes[:,0:2] - det_bboxes[:,3:4] * 0.5
277 |
278 | for i in range(det_bboxes.shape[0]):
279 | score = det_scores[i]
280 | box = det_bboxes[i,0:4]
281 | kps = []
282 | for kp in range(4):
283 | kps.append(det_bboxes[i,4+kp*2:6+kp*2])
284 | bodies.append(Body(float(score), box, kps))
285 | return bodies
286 |
287 |
288 | def non_max_suppression(bodies, nms_thresh):
289 |
290 | # cv2.dnn.NMSBoxes(boxes, scores, 0, nms_thresh) needs:
291 | # boxes = [ [x, y, w, h], ...] with x, y, w, h of type int
292 | # Currently, x, y, w, h are float between 0 and 1, so we arbitrarily multiply by 1000 and cast to int
293 | # boxes = [r.box for r in bodies]
294 | boxes = [ [int(x*1000) for x in r.pd_box] for r in bodies]
295 | scores = [r.pd_score for r in bodies]
296 | indices = cv2.dnn.NMSBoxes(boxes, scores, 0, nms_thresh)
297 | return [bodies[i[0]] for i in indices]
298 |
299 | def normalize_radians(angle):
300 | return angle - 2 * pi * floor((angle + pi) / (2 * pi))
301 |
302 | def rot_vec(vec, rotation):
303 | vx, vy = vec
304 | return [vx * cos(rotation) - vy * sin(rotation), vx * sin(rotation) + vy * cos(rotation)]
305 |
306 | def detections_to_rect(body, kp_pair=[0,1]):
307 | # https://github.com/google/mediapipe/blob/master/mediapipe/modules/pose_landmark/pose_detection_to_roi.pbtxt
308 | # # Converts pose detection into a rectangle based on center and scale alignment
309 | # # points. Pose detection contains four key points: first two for full-body pose
310 | # # and two more for upper-body pose.
311 | # node {
312 | # calculator: "SwitchContainer"
313 | # input_side_packet: "ENABLE:upper_body_only"
314 | # input_stream: "DETECTION:detection"
315 | # input_stream: "IMAGE_SIZE:image_size"
316 | # output_stream: "NORM_RECT:raw_roi"
317 | # options {
318 | # [mediapipe.SwitchContainerOptions.ext] {
319 | # contained_node: {
320 | # calculator: "AlignmentPointsRectsCalculator"
321 | # options: {
322 | # [mediapipe.DetectionsToRectsCalculatorOptions.ext] {
323 | # rotation_vector_start_keypoint_index: 0
324 | # rotation_vector_end_keypoint_index: 1
325 | # rotation_vector_target_angle_degrees: 90
326 | # }
327 | # }
328 | # }
329 | # contained_node: {
330 | # calculator: "AlignmentPointsRectsCalculator"
331 | # options: {
332 | # [mediapipe.DetectionsToRectsCalculatorOptions.ext] {
333 | # rotation_vector_start_keypoint_index: 2
334 | # rotation_vector_end_keypoint_index: 3
335 | # rotation_vector_target_angle_degrees: 90
336 | # }
337 | # }
338 | # }
339 | # }
340 | # }
341 | # }
342 |
343 | target_angle = pi * 0.5 # 90 = pi/2
344 |
345 | # AlignmentPointsRectsCalculator : https://github.com/google/mediapipe/blob/master/mediapipe/calculators/util/alignment_points_to_rects_calculator.cc
346 | x_center, y_center = body.pd_kps[kp_pair[0]]
347 | x_scale, y_scale = body.pd_kps[kp_pair[1]]
348 | # Bounding box size as double distance from center to scale point.
349 | box_size = sqrt((x_scale-x_center)**2 + (y_scale-y_center)**2) * 2
350 | body.rect_w = box_size
351 | body.rect_h = box_size
352 | body.rect_x_center = x_center
353 | body.rect_y_center = y_center
354 |
355 | rotation = target_angle - atan2(-(y_scale - y_center), x_scale - x_center)
356 | body.rotation = normalize_radians(rotation)
357 |
358 | def rotated_rect_to_points(cx, cy, w, h, rotation):
359 | b = cos(rotation) * 0.5
360 | a = sin(rotation) * 0.5
361 | points = []
362 | p0x = cx - a*h - b*w
363 | p0y = cy + b*h - a*w
364 | p1x = cx + a*h - b*w
365 | p1y = cy - b*h - a*w
366 | p2x = int(2*cx - p0x)
367 | p2y = int(2*cy - p0y)
368 | p3x = int(2*cx - p1x)
369 | p3y = int(2*cy - p1y)
370 | p0x, p0y, p1x, p1y = int(p0x), int(p0y), int(p1x), int(p1y)
371 | return [[p0x,p0y], [p1x,p1y], [p2x,p2y], [p3x,p3y]]
372 |
373 | def rect_transformation(body, w, h, scale = 1.25):
374 | """
375 | w, h : image input shape
376 | """
377 | # https://github.com/google/mediapipe/blob/master/mediapipe/modules/pose_landmark/pose_detection_to_roi.pbtxt
378 | # # Expands pose rect with marging used during training.
379 | # node {
380 | # calculator: "RectTransformationCalculator"
381 | # input_stream: "NORM_RECT:raw_roi"
382 | # input_stream: "IMAGE_SIZE:image_size"
383 | # output_stream: "roi"
384 | # options: {
385 | # [mediapipe.RectTransformationCalculatorOptions.ext] {
386 | # Version 0831:
387 | # scale_x: 1.5
388 | # scale_y: 1.5
389 | # Version 084:
390 | # scale_x: 1.25
391 | # scale_y: 1.25
392 | # square_long: true
393 | # }
394 | # }
395 | # }
396 | scale_x = scale
397 | scale_y = scale
398 | shift_x = 0
399 | shift_y = 0
400 |
401 | width = body.rect_w
402 | height = body.rect_h
403 | rotation = body.rotation
404 | if rotation == 0:
405 | body.rect_x_center_a = (body.rect_x_center + width * shift_x) * w
406 | body.rect_y_center_a = (body.rect_y_center + height * shift_y) * h
407 | else:
408 | x_shift = (w * width * shift_x * cos(rotation) - h * height * shift_y * sin(rotation))
409 | y_shift = (w * width * shift_x * sin(rotation) + h * height * shift_y * cos(rotation))
410 | body.rect_x_center_a = body.rect_x_center*w + x_shift
411 | body.rect_y_center_a = body.rect_y_center*h + y_shift
412 |
413 | # square_long: true
414 | long_side = max(width * w, height * h)
415 | body.rect_w_a = long_side * scale_x
416 | body.rect_h_a = long_side * scale_y
417 | body.rect_points = rotated_rect_to_points(body.rect_x_center_a, body.rect_y_center_a, body.rect_w_a, body.rect_h_a, body.rotation)
418 |
419 | def warp_rect_img(rect_points, img, w, h):
420 | src = np.array(rect_points[1:], dtype=np.float32) # rect_points[0] is left bottom point !
421 | dst = np.array([(0, 0), (w, 0), (w, h)], dtype=np.float32)
422 | mat = cv2.getAffineTransform(src, dst)
423 | return cv2.warpAffine(img, mat, (w, h))
424 |
425 | def distance(a, b):
426 | """
427 | a, b: 2 points in 3D (x,y,z)
428 | """
429 | return np.linalg.norm(a-b)
430 |
431 | def angle(a, b, c):
432 | # https://stackoverflow.com/questions/35176451/python-code-to-calculate-angle-between-three-point-using-their-3d-coordinates
433 | # a, b and c : points as np.array([x, y, z])
434 | ba = a - b
435 | bc = c - b
436 | cosine_angle = np.dot(ba, bc) / (np.linalg.norm(ba) * np.linalg.norm(bc))
437 | angle = np.arccos(cosine_angle)
438 |
439 | return np.degrees(angle)
440 |
441 | #
442 | def find_isp_scale_params(size, is_height=True):
443 | """
444 | Find closest valid size close to 'size' and and the corresponding parameters to setIspScale()
445 | This function is useful to work around a bug in depthai where ImageManip is scrambling images that have an invalid size
446 | is_height : boolean that indicates if the value is the height or the width of the image
447 | Returns: valid size, (numerator, denominator)
448 | """
449 | # We want size >= 288
450 | if size < 288:
451 | size = 288
452 |
453 | # We are looking for the list on integers that are divisible by 16 and
454 | # that can be written like n/d where n <= 16 and d <= 63
455 | if is_height:
456 | reference = 1080
457 | other = 1920
458 | else:
459 | reference = 1920
460 | other = 1080
461 | size_candidates = {}
462 | for s in range(288,reference,16):
463 | f = gcd(reference, s)
464 | n = s//f
465 | d = reference//f
466 | if n <= 16 and d <= 63 and int(round(other * n / d) % 2 == 0):
467 | size_candidates[s] = (n, d)
468 |
469 | # What is the candidate size closer to 'size' ?
470 | min_dist = -1
471 | for s in size_candidates:
472 | dist = abs(size - s)
473 | if min_dist == -1:
474 | min_dist = dist
475 | candidate = s
476 | else:
477 | if dist > min_dist: break
478 | candidate = s
479 | min_dist = dist
480 | return candidate, size_candidates[candidate]
481 |
482 | #
483 | # Filtering
484 | #
485 |
486 | class LandmarksSmoothingFilter:
487 | '''
488 | Adapted from: https://github.com/google/mediapipe/blob/master/mediapipe/calculators/util/landmarks_smoothing_calculator.cc
489 |
490 | frequency, min_cutoff, beta, derivate_cutoff:
491 | See class OneEuroFilter description.
492 | min_allowed_object_scale:
493 | If calculated object scale is less than given value smoothing will be
494 | disabled and landmarks will be returned as is. Default=1e-6
495 | disable_value_scaling:
496 | Disable value scaling based on object size and use `1.0` instead.
497 | If not disabled, value scale is calculated as inverse value of object
498 | size. Object size is calculated as maximum side of rectangular bounding
499 | box of the object in XY plane. Default=False
500 | '''
501 | def __init__(self,
502 | frequency=30,
503 | min_cutoff=1,
504 | beta=0,
505 | derivate_cutoff=1,
506 | min_allowed_object_scale=1e-6,
507 | disable_value_scaling=False
508 | ):
509 | self.frequency = frequency
510 | self.min_cutoff = min_cutoff
511 | self.beta = beta
512 | self.derivate_cutoff = derivate_cutoff
513 | self.min_allowed_object_scale = min_allowed_object_scale
514 | self.disable_value_scaling = disable_value_scaling
515 | self.init = True
516 |
517 | @staticmethod
518 | def get_object_scale(landmarks):
519 | # Estimate object scale to use its inverse value as velocity scale for
520 | # RelativeVelocityFilter. If value will be too small (less than
521 | # `options_.min_allowed_object_scale`) smoothing will be disabled and
522 | # landmarks will be returned as is.
523 | # Object scale is calculated as average between bounding box width and height
524 | # with sides parallel to axis.
525 | min_xy = np.min(landmarks[:,:2], axis=0)
526 | max_xy = np.max(landmarks[:,:2], axis=0)
527 | return np.mean(max_xy - min_xy)
528 |
529 | def apply(self, landmarks, timestamp, object_scale=0):
530 | # object_scale: in practice, we use the size of the rotated rectangle region.rect_w_a=region.rect_h_a
531 |
532 | # Initialize filters
533 | if self.init:
534 | self.filters = OneEuroFilter(self.frequency, self.min_cutoff, self.beta, self.derivate_cutoff)
535 | self.init = False
536 |
537 | # Get value scale as inverse value of the object scale.
538 | # If value is too small smoothing will be disabled and landmarks will be
539 | # returned as is.
540 | if self.disable_value_scaling:
541 | value_scale = 1
542 | else:
543 | object_scale = object_scale if object_scale else self.get_object_scale(landmarks)
544 | if object_scale < self.min_allowed_object_scale:
545 | return landmarks
546 | value_scale = 1 / object_scale
547 |
548 | return self.filters.apply(landmarks, value_scale, timestamp)
549 |
550 | def get_alpha(self, cutoff):
551 | '''
552 | te = 1.0 / self.frequency
553 | tau = 1.0 / (2 * Math.PI * cutoff)
554 | result = 1 / (1.0 + (tau / te))
555 | '''
556 | return 1.0 / (1.0 + (self.frequency / (2 * pi * cutoff)))
557 |
558 | def reset(self):
559 | self.init = True
560 |
561 | class OneEuroFilter:
562 | '''
563 | Adapted from: https://github.com/google/mediapipe/blob/master/mediapipe/util/filtering/one_euro_filter.cc
564 | Paper: https://cristal.univ-lille.fr/~casiez/1euro/
565 |
566 | frequency:
567 | Frequency of incoming frames defined in seconds. Used
568 | only if can't be calculated from provided events (e.g.
569 | on the very first frame). Default=30
570 | min_cutoff:
571 | Minimum cutoff frequency. Start by tuning this parameter while
572 | keeping `beta=0` to reduce jittering to the desired level. 1Hz
573 | (the default value) is a a good starting point.
574 | beta:
575 | Cutoff slope. After `min_cutoff` is configured, start
576 | increasing `beta` value to reduce the lag introduced by the
577 | `min_cutoff`. Find the desired balance between jittering and lag. Default=0
578 | derivate_cutoff:
579 | Cutoff frequency for derivate. It is set to 1Hz in the
580 | original algorithm, but can be turned to further smooth the
581 | speed (i.e. derivate) on the object. Default=1
582 | '''
583 | def __init__(self,
584 | frequency=30,
585 | min_cutoff=1,
586 | beta=0,
587 | derivate_cutoff=1,
588 | ):
589 | self.frequency = frequency
590 | self.min_cutoff = min_cutoff
591 | self.beta = beta
592 | self.derivate_cutoff = derivate_cutoff
593 | self.x = LowPassFilter(self.get_alpha(min_cutoff))
594 | self.dx = LowPassFilter(self.get_alpha(derivate_cutoff))
595 | self.last_timestamp = 0
596 |
597 | def get_alpha(self, cutoff):
598 | '''
599 | te = 1.0 / self.frequency
600 | tau = 1.0 / (2 * Math.PI * cutoff)
601 | result = 1 / (1.0 + (tau / te))
602 | '''
603 | return 1.0 / (1.0 + (self.frequency / (2 * pi * cutoff)))
604 |
605 | def apply(self, value, value_scale, timestamp):
606 | '''
607 | Applies filter to the value.
608 | timestamp in s associated with the value (for instance,
609 | timestamp of the frame where you got value from).
610 | '''
611 | if self.last_timestamp >= timestamp:
612 | # Results are unpreditable in this case, so nothing to do but return same value.
613 | return value
614 |
615 | # Update the sampling frequency based on timestamps.
616 | if self.last_timestamp != 0 and timestamp != 0:
617 | self.frequency = 1 / (timestamp - self.last_timestamp)
618 | self.last_timestamp = timestamp
619 |
620 | # Estimate the current variation per second.
621 | if self.x.has_last_raw_value():
622 | dvalue = (value - self.x.last_raw_value()) * value_scale * self.frequency
623 | else:
624 | dvalue = 0
625 | edvalue = self.dx.apply_with_alpha(dvalue, self.get_alpha(self.derivate_cutoff))
626 |
627 | # Use it to update the cutoff frequency
628 | cutoff = self.min_cutoff + self.beta * np.abs(edvalue)
629 |
630 | # filter the given value.
631 | return self.x.apply_with_alpha(value, self.get_alpha(cutoff))
632 |
633 | class LowPassFilter:
634 | '''
635 | Adapted from: https://github.com/google/mediapipe/blob/master/mediapipe/util/filtering/low_pass_filter.cc
636 | Note that 'value' can be a numpy array
637 | '''
638 | def __init__(self, alpha=0.9):
639 | self.alpha = alpha
640 | self.initialized = False
641 |
642 | def apply(self, value):
643 | if self.initialized:
644 | # Regular lowpass filter.
645 | # result = alpha * value + (1 - alpha) * stored_value;
646 | result = self.alpha * value + (1 - self.alpha) * self.stored_value
647 | else:
648 | result = value
649 | self.initialized = True
650 | self.raw_value = value
651 | self.stored_value = result
652 | return result
653 |
654 | def apply_with_alpha(self, value, alpha):
655 | self.alpha = alpha
656 | return self.apply(value)
657 |
658 | def has_last_raw_value(self):
659 | return self.initialized
660 |
661 | def last_raw_value(self):
662 | return self.raw_value
663 |
664 | def last_value(self):
665 | return self.stored_value
666 |
667 | def reset(self):
668 | self.initialized = False
669 |
670 |
671 |
--------------------------------------------------------------------------------
/models/convert_models.sh:
--------------------------------------------------------------------------------
1 | # This script has to be run from the docker container started by ./docker_tflite2tensorflow.sh
2 |
3 | source /opt/intel/openvino_2021/bin/setupvars.sh
4 |
5 | replace () { # Replace in file $1 occurences of the string $2 by $3
6 | sed "s/${2}/${3}/" $1 > tmpf
7 | mv tmpf $1
8 | }
9 |
10 | convert_model () {
11 | model_name=$1
12 | if [ -z "$2" ]
13 | then
14 | arg_mean_values=""
15 | else
16 | arg_mean_values="--mean_values ${2}"
17 | fi
18 | if [ -z "$3" ]
19 | then
20 | arg_scale_values=""
21 | else
22 | arg_scale_values="--scale_values ${3}"
23 | fi
24 | mean_values=$2
25 | scale_values=$3
26 | input_precision=$4
27 | tflite2tensorflow \
28 | --model_path ${model_name}.tflite \
29 | --model_output_path ${model_name} \
30 | --flatc_path ~/flatc \
31 | --schema_path ~/schema.fbs \
32 | --output_pb \
33 | --optimizing_for_openvino_and_myriad
34 | # For generating Openvino "non normalized input" models (the normalization would need to be made explictly in the code):
35 | #tflite2tensorflow \
36 | # --model_path ${model_name}.tflite \
37 | # --model_output_path ${model_name} \
38 | # --flatc_path ~/flatc \
39 | # --schema_path ~/schema.fbs \
40 | # --output_openvino_and_myriad
41 | # Generate Openvino "normalized input" models
42 | /opt/intel/openvino_2021/deployment_tools/model_optimizer/mo_tf.py \
43 | --saved_model_dir ${model_name} \
44 | --model_name ${model_name} \
45 | --data_type FP16 \
46 | ${arg_mean_values} \
47 | ${arg_scale_values} \
48 | --reverse_input_channels
49 | # For Interpolate layers, replace in coordinate_transformation_mode, "half_pixel" by "align_corners" (bug optimizer)
50 | if [ model_name != "pose_detection" ]
51 | then
52 | replace ${model_name}.xml half_pixel align_corners
53 | fi
54 |
55 | /opt/intel/openvino_2021/deployment_tools/inference_engine/lib/intel64/myriad_compile \
56 | -m ${model_name}.xml \
57 | -ip $input_precision \
58 | -VPU_NUMBER_OF_SHAVES 4 \
59 | -VPU_NUMBER_OF_CMX_SLICES 4 \
60 | -o ${model_name}_sh4.blob
61 | }
62 |
63 | convert_model pose_detection "[127.5,127.5,127.5]" "[127.5,127.5,127.5]" "u8"
64 | convert_model pose_landmark_full "" "" "fp16"
65 | convert_model pose_landmark_lite "" "" "fp16"
66 | convert_model pose_landmark_heavy "" "" "fp16"
67 |
68 |
69 |
--------------------------------------------------------------------------------
/models/gen_blob_shave.sh:
--------------------------------------------------------------------------------
1 | # This script has to be run from the docker container started by ./docker_tflite2tensorflow.sh
2 |
3 | usage ()
4 | {
5 | echo "Generate a new blob with a specified number of shaves and cmx (nb cmx = nb shaves)"
6 | echo "Usage: ${0} -m model -n nb_shaves"
7 | echo "model = 'pd' for pose detection, 'full' or 'lite' or 'heavy' for landmarks"
8 | echo "nb_shaves must be between 1 and 13"
9 | }
10 |
11 | while getopts ":hm:n:" opt; do
12 | case ${opt} in
13 | h )
14 | usage
15 | exit 0
16 | ;;
17 | m )
18 | model=$OPTARG
19 | ;;
20 | n )
21 | nb_shaves=$OPTARG
22 | ;;
23 | : )
24 | echo "Error: -$OPTARG requires an argument."
25 | usage
26 | exit 1
27 | ;;
28 | \? )
29 | echo "Invalid option: -$OPTARG"
30 | usage
31 | exit 1
32 | ;;
33 | esac
34 | done
35 | if [ -z "$model" ]
36 | then
37 | echo "Model not specified !"
38 | usage
39 | exit 1
40 | fi
41 | if [ $model == "pd" ]
42 | then
43 | model="pose_detection"
44 | input_precision="u8"
45 | elif [ $model == "full" ]
46 | then
47 | model="pose_landmark_full"
48 | input_precision="fp16"
49 | elif [ $model == "lite" ]
50 | then
51 | model="pose_landmark_lite"
52 | input_precision="fp16"
53 | elif [ $model == "heavy" ]
54 | then
55 | model="pose_landmark_heavy"
56 | input_precision="fp16"
57 | else
58 | echo "Invalid model !"
59 | usage
60 | exit 1
61 | fi
62 | model_xml="${model}.xml"
63 | if [ ! -f $model_xml ]
64 | then
65 | echo "The model ${model_xml} does not exist"
66 | echo "Have you run convert_models.sh ?"
67 | exit 1
68 | fi
69 | if [ -z "$nb_shaves" ]
70 | then
71 | echo "The number of shaves must be specified with -n"
72 | usage
73 | exit 1
74 | fi
75 | if [ $nb_shaves -lt 1 -o $nb_shaves -gt 13 ]
76 | then
77 | echo "Invalid number of shaves !"
78 | usage
79 | exit 1
80 | fi
81 |
82 | model_blob="${model}_sh${nb_shaves}.blob"
83 |
84 | echo Model: $model_xml $model_blob
85 | echo Shaves $nb_shaves
86 |
87 | source /opt/intel/openvino_2021/bin/setupvars.sh
88 |
89 |
90 | /opt/intel/openvino_2021/deployment_tools/inference_engine/lib/intel64/myriad_compile -m $model_xml -ip $input_precision -VPU_NUMBER_OF_SHAVES $nb_shaves -VPU_NUMBER_OF_CMX_SLICES $nb_shaves -o $model_blob
91 |
92 |
--------------------------------------------------------------------------------
/models/pose_detection_sh4.blob:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/models/pose_detection_sh4.blob
--------------------------------------------------------------------------------
/models/pose_landmark_full_sh4.blob:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/models/pose_landmark_full_sh4.blob
--------------------------------------------------------------------------------
/models/pose_landmark_heavy_sh4.blob:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/models/pose_landmark_heavy_sh4.blob
--------------------------------------------------------------------------------
/models/pose_landmark_lite_sh4.blob:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/geaxgx/depthai_blazepose/d453e85231e36fc6625a1fc654cc4b5c24344a85/models/pose_landmark_lite_sh4.blob
--------------------------------------------------------------------------------
/o3d_utils.py:
--------------------------------------------------------------------------------
1 | import open3d as o3d
2 | import numpy as np
3 |
4 |
5 | # From : https://stackoverflow.com/a/59026582/8574085
6 |
7 | def calculate_zy_rotation_for_arrow(v):
8 | """
9 | Calculates the rotations required to go from the vector v to the
10 | z axis vector. The first rotation that is
11 | calculated is over the z axis. This will leave the vector v on the
12 | XZ plane. Then, the rotation over the y axis.
13 |
14 | Returns the angles of rotation over axis z and y required to
15 | get the vector v into the same orientation as axis z
16 |
17 | Args:
18 | - v ():
19 | """
20 | # Rotation over z axis
21 | gamma = np.arctan(v[1]/v[0])
22 | Rz = np.array([[np.cos(gamma),-np.sin(gamma),0],
23 | [np.sin(gamma),np.cos(gamma),0],
24 | [0,0,1]])
25 | # Rotate v to calculate next rotation
26 | v = Rz.T@v.reshape(-1,1)
27 | v = v.reshape(-1)
28 | # Rotation over y axis
29 | beta = np.arctan(v[0]/v[2])
30 | Ry = np.array([[np.cos(beta),0,np.sin(beta)],
31 | [0,1,0],
32 | [-np.sin(beta),0,np.cos(beta)]])
33 | return Rz @ Ry
34 |
35 | def create_cylinder(height=1, radius=None, resolution=20):
36 | """
37 | Create an cylinder in Open3D
38 | """
39 | radius = height/20 if radius is None else radius
40 | mesh_frame = o3d.geometry.TriangleMesh.create_cylinder(
41 | radius=radius,
42 | height=height,
43 | resolution=resolution)
44 | return(mesh_frame)
45 |
46 | def create_segment(a, b, radius=0.05, color=(1,1,0), resolution=20):
47 | """
48 | Creates an line(cylinder) from an pointa to point b,
49 | or create an line from a vector v starting from origin.
50 | Args:
51 | - a, b: End points [x,y,z]
52 | - radius: radius cylinder
53 | """
54 | a = np.array(a)
55 | b = np.array(b)
56 | T = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
57 | T[:3, -1] = a
58 | v = b-a
59 |
60 | height = np.linalg.norm(v)
61 | if height == 0: return None
62 | R = calculate_zy_rotation_for_arrow(v)
63 | mesh = create_cylinder(height, radius)
64 | mesh.rotate(R, center=np.array([0, 0, 0]))
65 | mesh.translate((a+b)/2)
66 | mesh.paint_uniform_color(color)
67 | mesh.compute_vertex_normals()
68 | return mesh
69 |
70 | def create_tetra(p1, p2, p3, p4, color=(1,1,0)):
71 | vertices = o3d.utility.Vector3dVector([p1, p2, p3, p4])
72 | tetras = o3d.utility.Vector4iVector([[0, 1, 2, 3]])
73 | mesh = o3d.geometry.TetraMesh(vertices, tetras)
74 | mesh.paint_uniform_color(color)
75 | return mesh
76 |
77 | def create_grid(p0, p1, p2, p3, ni1, ni2, color=(0,0,0)):
78 | '''
79 | p0, p1, p2, p3 : points defining a quadrilateral
80 | ni1: nb of equidistant intervals on segments p0p1 and p3p2
81 | ni2: nb of equidistant intervals on segments p1p2 and p0p3
82 | '''
83 | p0 = np.array(p0)
84 | p1 = np.array(p1)
85 | p2 = np.array(p2)
86 | p3 = np.array(p3)
87 |
88 | vertices = [p0, p1, p2, p3]
89 | lines = [[0,1],[0,3],[1,2],[2,3]]
90 | for i in range(1,ni1):
91 | l = len(vertices)
92 | vertices.append((p0*(ni1-i)+p1*i)/ni1)
93 | vertices.append((p3*(ni1-i)+p2*i)/ni1)
94 | lines.append([l,l+1])
95 | for i in range(1,ni2):
96 | l = len(vertices)
97 | vertices.append((p1*(ni2-i)+p2*i)/ni2)
98 | vertices.append((p0*(ni2-i)+p3*i)/ni2)
99 | lines.append([l,l+1])
100 | vertices = o3d.utility.Vector3dVector(vertices)
101 | lines = o3d.utility.Vector2iVector(lines)
102 | mesh = o3d.geometry.LineSet(vertices, lines)
103 | mesh.paint_uniform_color(color)
104 | return mesh
105 |
106 |
107 | def create_coord_frame(origin=[0, 0, 0],size=1):
108 | mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=size)
109 | mesh.translate(origin)
110 | return mesh
111 |
112 |
113 | # Visu3d : custom class used to visualize 3d skeleton
114 | class Visu3D:
115 | def __init__(self, bg_color=[0,0,0], zoom=1, segment_radius=1):
116 | self.vis = o3d.visualization.VisualizerWithKeyCallback()
117 | self.vis.create_window()
118 | opt = self.vis.get_render_option()
119 | opt.background_color = np.asarray(bg_color)
120 | # Defining callbacks - Key codes: https://www.glfw.org/docs/latest/group__keys.html
121 | self.vis.register_key_callback(ord("R"), self.start_rotating)
122 | self.vis.register_key_callback(ord("O"), self.start_oscillating)
123 | self.vis.register_key_callback(ord("S"), self.stop_moving)
124 | self.vis.register_key_callback(262, self.turn_view_right) # Right arrow
125 | self.vis.register_key_callback(263, self.turn_view_left) # Left arrow
126 | self.vis.register_key_callback(265, self.incr_rot_speed) # Up arrow
127 | self.vis.register_key_callback(264, self.decr_rot_speed) # Down arrow
128 | self.view_control = self.vis.get_view_control()
129 | self.zoom = zoom
130 | self.segment_radius = segment_radius
131 | self.move = "oscillate"
132 | self.angle = 0
133 | self.direction = 1
134 | self.oscillate_angle = 200
135 | self.geometries = []
136 |
137 | def set_view(self):
138 | if self.angle_view % 4 == 0:
139 | ax = 0
140 | elif self.angle_view <= 3:
141 | ax = 1
142 | else:
143 | ax = -1
144 | if self.angle_view == 2 or self.angle_view == 6:
145 | az = 0
146 | elif 3 <= self.angle_view <= 5:
147 | az = 1
148 | else:
149 | az = -1
150 | self.view_control.set_front(np.array([ax,0,az]))
151 | self.view_control.set_up(np.array([0,-1,0]))
152 |
153 | def init_view(self):
154 | self.angle_view = 0
155 | self.rot_speed = 2
156 | self.set_view()
157 | self.view_control.set_zoom(self.zoom)
158 |
159 | def create_grid(self, p0, p1, p2, p3, ni1, ni2, color=(1,1,1)):
160 | '''
161 | p0, p1, p2, p3 : points defining a quadrilateral
162 | ni1: nb of equidistant intervals on segments p0p1 and p3p2
163 | ni2: nb of equidistant intervals on segments p1p2 and p0p3
164 | '''
165 | grid = create_grid(p0, p1, p2, p3, ni1, ni2, color)
166 | self.vis.add_geometry(grid)
167 | self.geometries.append(grid)
168 |
169 | def create_camera(self):
170 | cam = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.02, cone_radius=0.03, cylinder_height=0.1, cone_height=0.08)
171 | cam.paint_uniform_color([0.2,0.7,1])
172 | cam.compute_vertex_normals()
173 | self.geometries.append(cam)
174 |
175 | def add_geometries(self):
176 | for geo in self.geometries:
177 | self.vis.add_geometry(geo, reset_bounding_box=False)
178 |
179 | def add_segment(self, p1, p2, radius=None, color=[1,1,1]):
180 | radius = self.segment_radius if radius is None else radius
181 | line = create_segment(p1, p2, radius=radius, color=color)
182 | if line: self.vis.add_geometry(line, reset_bounding_box=False)
183 |
184 | def clear(self):
185 | self.vis.clear_geometries()
186 |
187 | # Callback
188 | def incr_rot_speed(self, vis):
189 | if self.move == "rotate":
190 | if self.rot_speed * self.direction == -1:
191 | self.direction = 1
192 | else:
193 | self.rot_speed += self.direction
194 | else:
195 | self.rot_speed += 1
196 | # Callback
197 | def decr_rot_speed(self, vis):
198 | if self.move == "rotate":
199 | if self.rot_speed * self.direction == 1:
200 | self.direction = -1
201 | else:
202 | self.rot_speed -= self.direction
203 | else:
204 | self.rot_speed = max (1, self.rot_speed-1)
205 | # Callback
206 | def turn_view_right(self, vis):
207 | self.angle_view = (self.angle_view + 1) %8
208 | self.set_view()
209 | self.move = None
210 | # Callback
211 | def turn_view_left(self, vis):
212 | self.angle_view = (self.angle_view - 1) %8
213 | self.set_view()
214 | self.move = None
215 | # Callback
216 | def start_rotating(self, vis):
217 | self.move = "rotate"
218 | # Callback
219 | def start_oscillating(self, vis):
220 | self.move = "oscillate"
221 | self.angle = 0
222 | # Callback
223 | def stop_moving(self, vis):
224 | self.move = None
225 |
226 | def try_move(self):
227 | if self.move == "rotate":
228 | self.view_control.rotate(self.rot_speed * self.direction,0)
229 | elif self.move == "oscillate":
230 | self.view_control.rotate(self.rot_speed * self.direction,0)
231 | self.angle += self.rot_speed * self.direction
232 | if abs(self.angle) >= self.oscillate_angle:
233 | self.direction = - self.direction
234 |
235 | def render(self):
236 | self.vis.poll_events()
237 | self.vis.update_renderer()
238 |
239 |
240 |
241 | if __name__ == "__main__":
242 |
243 | line = create_segment([0, 0, 0], [1, 0, 0], color=(1,0,0))
244 | line2 = create_segment([1, 0, 0], [1, 1, 0], color=(0,1,0))
245 | line3 = create_segment([1, 1, 0], [0, 0, 0], radius=0.1)
246 | grid = create_grid([0,0,0],[0,0,1],[0,1,1],[0,1,0], 3, 2)
247 | frame =create_coord_frame()
248 | print(grid)
249 | # Draw everything
250 | o3d.visualization.draw_geometries([line, line2, line3, grid, frame])
--------------------------------------------------------------------------------
/requirements.txt:
--------------------------------------------------------------------------------
1 | opencv-python >= 4.5.1.48
2 | open3d
3 | depthai >= 2.10
4 |
--------------------------------------------------------------------------------
/template_manager_script.py:
--------------------------------------------------------------------------------
1 | """
2 | This file is the template of the scripting node source code in edge mode
3 | Substitution is made in BlazeposeDepthaiEdge.py
4 |
5 | In the following:
6 | rrn_ : normalized [0:1] coordinates in rotated rectangle coordinate systems
7 | sqn_ : normalized [0:1] coordinates in squared input image
8 | """
9 |
10 | ${_TRACE} ("Starting manager script node")
11 |
12 | import marshal
13 | from math import sin, cos, atan2, pi, hypot, degrees, floor
14 |
15 | # Indexes of some keypoints
16 | left_shoulder = 11
17 | right_shoulder = 12
18 | left_hip = 23
19 | right_hip = 24
20 |
21 | ${_IF_XYZ}
22 | # We use a filter for smoothing the reference point (mid hips or mid shoulders)
23 | # for which we fetch the (x,y,z). Without this filter, the reference point is very shaky
24 | class SmoothingFilter:
25 | def __init__(self, alpha):
26 | self.alpha = alpha
27 | self.initialized = False
28 | def apply(self, x):
29 | if self.initialized:
30 | result = self.alpha * x + (1 - self.alpha) * self.prev_x
31 | else:
32 | result = x
33 | self.initialized = True
34 | self.prev_x = result
35 | return int(result)
36 | def reset(self):
37 | self.initialized = False
38 |
39 | filter_x = SmoothingFilter(0.5)
40 | filter_y = SmoothingFilter(0.5)
41 | ${_IF_XYZ}
42 |
43 | # BufferMgr is used to statically allocate buffers once
44 | # (replace dynamic allocation).
45 | # These buffers are used for sending result to host
46 | class BufferMgr:
47 | def __init__(self):
48 | self._bufs = {}
49 | def __call__(self, size):
50 | try:
51 | buf = self._bufs[size]
52 | except KeyError:
53 | buf = self._bufs[size] = Buffer(size)
54 | ${_TRACE} (f"New buffer allocated: {size}")
55 | return buf
56 |
57 | buffer_mgr = BufferMgr()
58 |
59 | def send_result(type, lm_score=0, rect_center_x=0, rect_center_y=0, rect_size=0, rotation=0, lms=0, lms_world=0, xyz_ref=0, xyz=0, xyz_zone=0):
60 | # type : 0, 1 or 2
61 | # 0 : pose detection only (detection score < threshold)
62 | # 1 : pose detection + landmark regression
63 | # 2 : landmark regression only (ROI computed from previous landmarks)
64 | result = dict([("type", type), ("lm_score", lm_score), ("rotation", rotation),
65 | ("rect_center_x", rect_center_x), ("rect_center_y", rect_center_y), ("rect_size", rect_size),
66 | ("lms", lms), ('lms_world', lms_world),
67 | ("xyz_ref", xyz_ref), ("xyz", xyz), ("xyz_zone", xyz_zone)])
68 | result_serial = marshal.dumps(result, 2)
69 | buffer = buffer_mgr(len(result_serial))
70 | buffer.getData()[:] = result_serial
71 | ${_TRACE} ("len result:"+str(len(result_serial)))
72 |
73 | node.io['host'].send(buffer)
74 | ${_TRACE} ("Manager sent result to host")
75 |
76 |
77 | def rr2img(rrn_x, rrn_y):
78 | # Convert a point (rrn_x, rrn_y) expressed in normalized rotated rectangle (rrn)
79 | # into (X, Y) expressed in normalized image (sqn)
80 | X = sqn_rr_center_x + sqn_rr_size * ((rrn_x - 0.5) * cos_rot + (0.5 - rrn_y) * sin_rot)
81 | Y = sqn_rr_center_y + sqn_rr_size * ((rrn_y - 0.5) * cos_rot + (rrn_x - 0.5) * sin_rot)
82 | return X, Y
83 |
84 | norm_pad_size = ${_pad_h} / ${_frame_size}
85 |
86 | def is_visible(lm_id):
87 | # Is the landmark lm_id is visible ?
88 | # Here visibility means inferred visibility from the landmark model
89 | return lms[lm_id*5+3] > ${_visibility_threshold}
90 |
91 | def is_in_image(sqn_x, sqn_y):
92 | # Is the point (sqn_x, sqn_y) is included in the useful part of the image (excluding the pads)?
93 | return norm_pad_size < sqn_y < 1 - norm_pad_size
94 |
95 | # send_new_frame_to_branch defines on which branch new incoming frames are sent
96 | # 1 = pose detection branch
97 | # 2 = landmark branch
98 | send_new_frame_to_branch = 1
99 |
100 | next_roi_lm_idx = 33*5
101 |
102 | cfg_pre_pd = ImageManipConfig()
103 | cfg_pre_pd.setResizeThumbnail(224, 224, 0, 0, 0)
104 |
105 | while True:
106 | if send_new_frame_to_branch == 1: # Routing frame to pd
107 | node.io['pre_pd_manip_cfg'].send(cfg_pre_pd)
108 | ${_TRACE} ("Manager sent thumbnail config to pre_pd manip")
109 | # Wait for pd post processing's result
110 | detection = node.io['from_post_pd_nn'].get().getLayerFp16("result")
111 | ${_TRACE} ("Manager received pd result: "+str(detection))
112 | pd_score, sqn_rr_center_x, sqn_rr_center_y, sqn_scale_x, sqn_scale_y = detection
113 | if pd_score < ${_pd_score_thresh}:
114 | send_result(0)
115 | continue
116 | scale_center_x = sqn_scale_x - sqn_rr_center_x
117 | scale_center_y = sqn_scale_y - sqn_rr_center_y
118 | sqn_rr_size = 2 * ${_rect_transf_scale} * hypot(scale_center_x, scale_center_y)
119 | rotation = 0.5 * pi - atan2(-scale_center_y, scale_center_x)
120 | rotation = rotation - 2 * pi *floor((rotation + pi) / (2 * pi))
121 | ${_IF_XYZ}
122 | filter_x.reset()
123 | filter_y.reset()
124 | ${_IF_XYZ}
125 |
126 |
127 | # Routing frame to lm
128 | sin_rot = sin(rotation) # We will often need these values later
129 | cos_rot = cos(rotation)
130 | # Tell pre_lm_manip how to crop body region
131 | rr = RotatedRect()
132 | rr.center.x = sqn_rr_center_x
133 | rr.center.y = (sqn_rr_center_y * ${_frame_size} - ${_pad_h}) / ${_img_h}
134 | rr.size.width = sqn_rr_size
135 | rr.size.height = sqn_rr_size * ${_frame_size} / ${_img_h}
136 | rr.angle = degrees(rotation)
137 | cfg = ImageManipConfig()
138 | cfg.setCropRotatedRect(rr, True)
139 | cfg.setResize(256, 256)
140 | node.io['pre_lm_manip_cfg'].send(cfg)
141 | ${_TRACE} ("Manager sent config to pre_lm manip")
142 |
143 | # Wait for lm's result
144 | lm_result = node.io['from_lm_nn'].get()
145 | ${_TRACE} ("Manager received result from lm nn")
146 | lm_score = lm_result.getLayerFp16("Identity_1")[0]
147 | if lm_score > ${_lm_score_thresh}:
148 | lms = lm_result.getLayerFp16("Identity")
149 | lms_world = lm_result.getLayerFp16("Identity_4")[:99]
150 |
151 | xyz = 0
152 | xyz_zone = 0
153 | xyz_ref = 0
154 | # Query xyz
155 | ${_IF_XYZ}
156 | # Choosing the reference point: mid hips if hips visible, or mid shoulders otherwise
157 | # xyz_ref codes the reference point, 1 if mid hips, 2 if mid shoulders, 0 if no reference point
158 | if is_visible(right_hip) and is_visible(left_hip):
159 | kp1 = right_hip
160 | kp2 = left_hip
161 | rrn_xyz_ref_x = (lms[5*kp1] + lms[5*kp2]) / 512 # 512 = 256*2 (256 for normalizing, 2 for the mean)
162 | rrn_xyz_ref_y = (lms[5*kp1+1] + lms[5*kp2+1]) / 512
163 | sqn_xyz_ref_x, sqn_xyz_ref_y = rr2img(rrn_xyz_ref_x, rrn_xyz_ref_y)
164 | if is_in_image(sqn_xyz_ref_x, sqn_xyz_ref_y):
165 | xyz_ref = 1
166 | if xyz_ref == 0 and is_visible(right_shoulder) and is_visible(left_shoulder):
167 | kp1 = right_shoulder
168 | kp2 = left_shoulder
169 | rrn_xyz_ref_x = (lms[5*kp1] + lms[5*kp2]) / 512 # 512 = 256*2 (256 for normalizing, 2 for the mean)
170 | rrn_xyz_ref_y = (lms[5*kp1+1] + lms[5*kp2+1]) / 512
171 | sqn_xyz_ref_x, sqn_xyz_ref_y = rr2img(rrn_xyz_ref_x, rrn_xyz_ref_y)
172 | if is_in_image(sqn_xyz_ref_x, sqn_xyz_ref_y):
173 | xyz_ref = 2
174 | if xyz_ref:
175 | cfg = SpatialLocationCalculatorConfig()
176 | conf_data = SpatialLocationCalculatorConfigData()
177 | conf_data.depthThresholds.lowerThreshold = 100
178 | conf_data.depthThresholds.upperThreshold = 10000
179 | half_zone_size = max(int(sqn_rr_size * ${_frame_size} / 90), 4)
180 | xc = filter_x.apply(sqn_xyz_ref_x * ${_frame_size} + ${_crop_w})
181 | yc = filter_y.apply(sqn_xyz_ref_y * ${_frame_size} - ${_pad_h})
182 | roi_left = max(0, xc - half_zone_size)
183 | roi_right = min(${_img_w}-1, xc + half_zone_size)
184 | roi_top = max(0, yc - half_zone_size)
185 | roi_bottom = min(${_img_h}-1, yc + half_zone_size)
186 | roi_topleft = Point2f(roi_left, roi_top)
187 | roi_bottomright = Point2f(roi_right, roi_bottom)
188 | conf_data.roi = Rect(roi_topleft, roi_bottomright)
189 | cfg = SpatialLocationCalculatorConfig()
190 | cfg.addROI(conf_data)
191 | node.io['spatial_location_config'].send(cfg)
192 | ${_TRACE} ("Manager sent ROI to spatial_location_config")
193 |
194 | # Wait xyz response
195 | xyz_data = node.io['spatial_data'].get().getSpatialLocations()
196 | ${_TRACE} ("Manager received spatial_location")
197 | coords = xyz_data[0].spatialCoordinates
198 | xyz = [float(coords.x), float(coords.y), float(coords.z)]
199 | roi = xyz_data[0].config.roi
200 | xyz_zone = [int(roi.topLeft().x - ${_crop_w}), int(roi.topLeft().y), int(roi.bottomRight().x - ${_crop_w}), int(roi.bottomRight().y)]
201 | else:
202 | xyz = [0.0] * 3
203 | xyz_zone = [0] * 4
204 | ${_IF_XYZ}
205 |
206 | # Send result to host
207 | send_result(send_new_frame_to_branch, lm_score, sqn_rr_center_x, sqn_rr_center_y, sqn_rr_size, rotation, lms, lms_world, xyz_ref, xyz, xyz_zone)
208 |
209 | if not ${_force_detection}:
210 | send_new_frame_to_branch = 2
211 | # Calculate the ROI for next frame
212 | rrn_rr_center_x = lms[next_roi_lm_idx] / 256
213 | rrn_rr_center_y = lms[next_roi_lm_idx+1] / 256
214 | rrn_scale_x = lms[next_roi_lm_idx+5] / 256
215 | rrn_scale_y = lms[next_roi_lm_idx+6] / 256
216 | sqn_scale_x, sqn_scale_y = rr2img(rrn_scale_x, rrn_scale_y)
217 | sqn_rr_center_x, sqn_rr_center_y = rr2img(rrn_rr_center_x, rrn_rr_center_y)
218 | scale_center_x = sqn_scale_x - sqn_rr_center_x
219 | scale_center_y = sqn_scale_y - sqn_rr_center_y
220 | sqn_rr_size = 2 * ${_rect_transf_scale} * hypot(scale_center_x, scale_center_y)
221 | rotation = 0.5 * pi - atan2(-scale_center_y, scale_center_x)
222 | rotation = rotation - 2 * pi *floor((rotation + pi) / (2 * pi))
223 |
224 | else:
225 | send_result(send_new_frame_to_branch, lm_score)
226 | send_new_frame_to_branch = 1
227 |
--------------------------------------------------------------------------------