├── .ipynb_checkpoints └── example-checkpoint.ipynb ├── LICENSE ├── README.md ├── data ├── sample_img.pkl └── sample_pose.pkl ├── example.ipynb ├── kinect_preprocess_example.py ├── kinect_smoothing ├── __init__.py ├── __pycache__ │ ├── __init__.cpython-36.pyc │ ├── coordinate_calculation.cpython-36.pyc │ ├── depth_image_smoothing.cpython-36.pyc │ ├── trajectory_smoothing.cpython-36.pyc │ └── utils.cpython-36.pyc ├── coordinate_calculation.py ├── depth_image_smoothing.py ├── pipeline.py ├── trajectory_smoothing.py └── utils.py └── requirements.txt /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Intelligent Control Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kinect Depth Image Smoothing 2 | - "Kinect Smoothing" helps you to smooth and filter the Kinect depth image and trajectory data. 3 | 4 | ## How to Use it 5 | * Installation 6 | ``` 7 | git clone https://github.com/intelligent-control-lab/Kinect_Smoothing.git 8 | pip install -r requirements.txt 9 | ``` 10 | * Running 11 | 12 | Please check [example.ipynb](example.ipynb) and [kinect_preprocess_example.py](kinect_preprocess_example.py). 13 | 14 | * Data preparation 15 | We saved many frames of depth images in `data/sample_img.pkl` and saved corresponding frames of position coordinates in `data/sample_pose.pkl ` 16 | `e.g. sample_img = [ [ image_1 ] , [ image_2 ], ... , [ image_t ] ].` each `image_i` has a shape of `(width, height)`. 17 | In case if anyone wants to use it for multiple files, then the code below should work. 18 | ``` 19 | import joblib 20 | import cv2 21 | import glob 22 | 23 | X_data = [] 24 | file_lists = glob.glob("/*.bmp") # image path 25 | for files in sorted(file_lists): 26 | if files.endswith(".bmp"): 27 | image = cv2.imread(files) 28 | X_data.append(image) 29 | joblib.dump(X_data, 'image_frames.pkl') 30 | ``` 31 | 32 | ## Features 33 | 34 | 1 . Depth Image Smoothing 35 | * Hole Filling Filter 36 | 37 | In the original Kinect depth image, there are many invalid pixels (they appear black on the image and are registered as 0's). This function helps you to fill the invalid values with based on the valid pixels in the vicinity. The methods for hole filling are as follows: 38 | * "min": Fill in with the neighboring minimum valid value 39 | * "max": Fill in with the neighboring maximum valid value, refer to [A computationally efficient denoising and hole-filling method for depth image enhancement](https://webpages.uncc.edu/cchen62/SPIE2016.pdf) 40 | * "mean": Fill in with the average of all neighboring valid values 41 | * "mode": Fill in with the mode of neighboring valid values, refer to [Smoothing Kinect Depth Frames in Real-Time](https://www.codeproject.com/Articles/317974/KinectDepthSmoothing) 42 | * "fmi": Fast Matching Inpainting, refer to [An Image Inpainting Technique Based on the Fast Marching Method](https://www.rug.nl/research/portal/files/14404904/2004JGraphToolsTelea.pdf) 43 | * "ns": Fluid Dynamics Inpainting, refer to [Navier-Stokes, Fluid Dynamics, and Image and Video Inpainting](https://conservancy.umn.edu/bitstream/handle/11299/3607/1772.pdf?sequence=1) 44 | 45 | * Denoising Filter 46 | 47 | After pixel filling, Denoising filters can be used to improve the resolution of the depth image. The methods for denoising filling are as follows: 48 | * "modeling": filter with Kinect V2 noise model, refer to [Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking](http://users.cecs.anu.edu.au/~nguyen/papers/conferences/Nguyen2012-ModelingKinectSensorNoise.pdf) 49 | * "modeling_pf": another Kinect V2 noise modeling by Peter Fankhauser [Kinect v2 for Mobile Robot Navigation: Evaluation and Modeling](https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/104272/1/eth-48073-01.pdf) 50 | * "anisotropic": smoothing with anisotropic filtering [Scale-space and edge detection using anisotropic diffusion](https://authors.library.caltech.edu/6498/1/PERieeetpami90.pdf) 51 | * "gaussian": smoothing with Gaussian filtering 52 | 53 | 2 . Trajectory Smoothing 54 | * Crop Filter: 55 | 56 | The x, y coordinates of the trajectory were captured by some object detection algorithms (e.g. Openpose). Sometimes the object will be positioned on the background, and the depth coordinates might register as invalid values on the Kinect. The Crop-Filter crops the invalid value and run some interpolation methods to replace it. The methods for invalid value replacement are as follows: 57 | * conventional interpolation methods: such as "zero","linear","slinear","quadratic","cubic","previous","next","nearest". 58 | * "pchip": PCHIP 1-d monotonic cubic interpolation, refer to [Monotone Piecewise Cubic Interpolation](https://epubs.siam.org/doi/pdf/10.1137/0717021?casa_token=IcEKTOT2mfgAAAAA:Ymwhtl0E5xdPakjEyhIuTAS5R5MQKUu3JrdLeo1Lu0qU8IMtDoX99RGwU2Ll4saxj68nVpLaVLQ) 59 | * "akima": Akima 1D Interpolator, refer to [A new method of interpolation and smooth curve fitting based on local procedures](http://200.17.213.49/lib/exe/fetch.php/wiki:internas:biblioteca:akima.pdf) 60 | 61 | * Gradient Crop Filter: 62 | 63 | Similar to Crop-Filter, the GradientCrop_Filter crops the large gradient values between near pixels maybe miss-labeled as background. The methods for invalid value replacement are as follows: 64 | * conventional interpolation methods: such as "zero","linear","slinear","quadratic","cubic","previous","next","nearest". 65 | * "pchip": PCHIP 1-d monotonic cubic interpolation. 66 | * "akima": Akima 1D Interpolator. 67 | 68 | 69 | * Smooth Filter: 70 | 71 | Smooth the data with a specific filter, which is effectively reducing the anomaly in the trajectory series. 72 | The methods for smoothing filter are as follows: 73 | * "kalman": smooth the signal with Kalman filter, refer to [Fundamentals of Kalman Filtering](http://iaac.technion.ac.il/workshops/2010/KFhandouts/LectKF1.pdf) 74 | * "wiener": smooth the signal with Wiener filter 75 | * "median": smooth the signal with median filter 76 | * "moving_average": smooth the signal with moving average filter 77 | * "exponential_moving_average": smooth the signal with exponential moving average filter 78 | 79 | * Motion Sampler: 80 | 81 | Motion detection and filtering out the stationary part of the trajectory. (Significant movements are preserved.) 82 | 83 | 3 . Real World Coordinate Calculation 84 | * Coordinate Calculator 85 | 86 | Transforming the pixel level coordinate of Kinect image to the real world coordinate. 87 | 88 | -------------------------------------------------------------------------------- /data/sample_img.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/data/sample_img.pkl -------------------------------------------------------------------------------- /data/sample_pose.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/data/sample_pose.pkl -------------------------------------------------------------------------------- /kinect_preprocess_example.py: -------------------------------------------------------------------------------- 1 | import os 2 | from time import time 3 | import joblib 4 | from glob import glob 5 | from multiprocessing import Pool 6 | from functools import partial 7 | 8 | from kinect_smoothing import HoleFilling_Filter, Denoising_Filter 9 | from kinect_smoothing import Crop_Filter, Smooth_Filter, Motion_Sampler 10 | from kinect_smoothing import Coordinate_Calculator 11 | from kinect_smoothing import Kinect_Openpose_Pipeline 12 | 13 | def simple_pipeline(image_frame,pose_frame): 14 | pipeline = Kinect_Openpose_Pipeline() 15 | real_coordinate = pipeline(image_frame,pose_frame) 16 | return real_coordinate 17 | 18 | def standard_pipeline(image_frame,pose_frame): 19 | hole_filter = HoleFilling_Filter(flag='min') 20 | image_frame = hole_filter.smooth_image_frames(image_frame) 21 | noise_filter = Denoising_Filter(flag='modeling', theta=60) 22 | image_frame = noise_filter.smooth_image_frames(image_frame) 23 | 24 | pose_filter = Crop_Filter(flag='pchip') 25 | pose_frame = pose_filter.smooth_multi_trajectories(pose_frame) 26 | 27 | calcutator = Coordinate_Calculator() 28 | raw_pose = calcutator.get_depth_coordinate(image_frame, pose_frame) 29 | raw_pose = pose_filter.smooth_multi_trajectories(raw_pose) 30 | 31 | smooth_filter = Smooth_Filter() 32 | raw_pose = smooth_filter.smooth_multi_trajectories(raw_pose) 33 | real_pose = calcutator.convert_real_coordinate(raw_pose) 34 | 35 | motion_filter = Motion_Sampler(motion_threshold=15, min_time_step=30) 36 | real_pose = motion_filter.motion_detection(real_pose) 37 | 38 | return real_pose 39 | 40 | def kinect_preprocess(img_path, pose_save_dir): 41 | t1 = time() 42 | img_path = img_path.replace('\\', '/') 43 | image_frame = joblib.load(img_path) 44 | pose_path = img_path.replace('img', 'pose') 45 | pose_frame = joblib.load(pose_path) 46 | 47 | real_coordinate = standard_pipeline(image_frame,pose_frame) 48 | #real_coordinate = simple_pipeline(image_frame,pose_frame) 49 | 50 | action, file_name = pose_path.split('/')[:-2] 51 | new_dir = os.path.join(pose_save_dir, action) 52 | real_new_path = os.path.join(new_dir, file_name) 53 | if not os.path.exists(new_dir): 54 | os.mkdir(new_dir) 55 | joblib.dump(real_coordinate, real_new_path, compress=3, protocol=2) 56 | print('preprocessed image %s, time-cost %f s' % (img_path, time() - t1)) 57 | 58 | def kicet_preprocess_multi(data_dir, pose_save_dir, num_thread=8): 59 | img_files = glob(data_dir + '*img.pkl') 60 | print('using multiprocess to preprare smoothing image', num_thread) 61 | pool = Pool(num_thread) 62 | pool.map(partial(kinect_preprocess,pose_save_dir=pose_save_dir),img_files) 63 | print('Processing Done') 64 | 65 | if __name__ == '__main__': 66 | data_dir = 'data/' 67 | pose_save_dir = 'save_data/' 68 | if not os.path.exists(pose_save_dir): 69 | os.mkdir(pose_save_dir) 70 | kicet_preprocess_multi(data_dir, pose_save_dir, num_thread=8) -------------------------------------------------------------------------------- /kinect_smoothing/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from .depth_image_smoothing import HoleFilling_Filter, Denoising_Filter 3 | from .trajectory_smoothing import Crop_Filter, Smooth_Filter, Motion_Sampler, GradientCrop_Filter 4 | from .coordinate_calculation import Coordinate_Calculator 5 | from .pipeline import Kinect_Openpose_Pipeline 6 | -------------------------------------------------------------------------------- /kinect_smoothing/__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/kinect_smoothing/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /kinect_smoothing/__pycache__/coordinate_calculation.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/kinect_smoothing/__pycache__/coordinate_calculation.cpython-36.pyc -------------------------------------------------------------------------------- /kinect_smoothing/__pycache__/depth_image_smoothing.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/kinect_smoothing/__pycache__/depth_image_smoothing.cpython-36.pyc -------------------------------------------------------------------------------- /kinect_smoothing/__pycache__/trajectory_smoothing.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/kinect_smoothing/__pycache__/trajectory_smoothing.cpython-36.pyc -------------------------------------------------------------------------------- /kinect_smoothing/__pycache__/utils.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intelligent-control-lab/Kinect_Smoothing/6801c154a2fde59459d2a82cfbd8b5562d302f77/kinect_smoothing/__pycache__/utils.cpython-36.pyc -------------------------------------------------------------------------------- /kinect_smoothing/coordinate_calculation.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | class Coordinate_Calculator(object): 5 | """ 6 | Convert the pixel level coordinate of Kinect to the real world coordinate. 7 | """ 8 | def __init__(self,image_width=512,image_height=424,fov_x=70.6,fov_y=60, 9 | neighbor_size=3,foreground_max_depth=1200,max_neighbor_size=10, 10 | image_pose_delay=0): 11 | """ 12 | :param image_width: int, width of depth image 13 | :param image_height: int, height of depth image 14 | :param fov_x: float, horizontal fields of view of depth image 15 | :param fov_y: float, vertical fields of view of depth image 16 | :param neighbor_size: int, radius of the neighboring area used for extract depth coordinate 17 | :param foreground_max_depth: float, max foreground depth for the depth extraction. 18 | if depth(x,y) > foreground_max_depth, that means (x,y) is in background, 19 | Then consider the valid depth in neighboring area, 20 | valid depth = min(depth[x-neighbor_size:x+neighbor_size,y-neighbor_size:y+neighbor_size]) 21 | :param max_neighbor_size: int, maximum radius of neighboring area 22 | :param image_pose_delay: int, the delay between image and pose frames 23 | """ 24 | self.width = image_width 25 | self.height = image_height 26 | self.fov_x = fov_x #fields of view 27 | self.fov_y = fov_y 28 | self.f_x = (self.width / 2) / math.tan(0.5 * math.pi * self.fov_x / 180) #focal length of the Kinect camera 29 | self.f_y = (self.height / 2) / math.tan(0.5 * math.pi * self.fov_y / 180) 30 | self.neighbor_size = neighbor_size 31 | self.foreground_max_depth = foreground_max_depth 32 | self.max_neighbor_size = max_neighbor_size 33 | self.image_pose_delay = image_pose_delay 34 | 35 | def cal_real_coordinate(self,x,y,z): 36 | """ 37 | calculate the real coordinate from pixel level (x,y) and real depth z 38 | :param x: int or float, pixel level x 39 | :param y: int or float, pixel level y 40 | :param z: int or float, real depth 41 | :return: (real_x, real_y,z),tuple of real word coordinates 42 | """ 43 | real_y = (self.height / 2 - y) * z / self.f_y 44 | real_x = (x - self.width / 2) * z / self.f_x 45 | return real_x, real_y, z 46 | 47 | def get_depth_coordinate(self,image_frames, position_frames): 48 | """ 49 | get depth value from depth-image-frames (from Kinect) and position frames (from Openpose) 50 | :param image_frames: list of numpy-array, [img_0, img_1,...], img_i has a shape of (height, width) 51 | :param position_frames: list of numpy-array, [pose_0,pose_1,...], pose_i have a shape of (joint_num, 2) 52 | :return: numpy array, calculated positions. have a shape of (time_step,joint_num,3) 53 | for each coordinate (x,y,z), x and y means the pixel level coordinate, and z is real depth 54 | """ 55 | delay = self.image_pose_delay 56 | raw_pose = [] 57 | if delay>0: 58 | image_frames = image_frames[:-delay] 59 | position_frames = position_frames[delay:] 60 | elif delay<0: 61 | image_frames = image_frames[-delay:] 62 | position_frames = position_frames[:delay] 63 | for img, poses in zip(image_frames, position_frames): 64 | raw_temp = [] 65 | for x, y in poses: 66 | _radius = self.neighbor_size 67 | z = 9999999999 68 | while z >= self.foreground_max_depth and _radius <= self.max_neighbor_size: 69 | lower_y, lower_x = max(0, y - _radius), max(0, x - _radius) 70 | z_array = img[lower_y:y + _radius + 1, lower_x:x + _radius + 1] 71 | z = np.min(z_array) 72 | _radius += 1 73 | raw_temp.append(np.array([x, y, z], dtype=np.float32)) 74 | raw_pose.append(raw_temp) 75 | raw_pose = np.array(raw_pose) 76 | return raw_pose 77 | 78 | def convert_real_coordinate(self,raw_coordinate): 79 | """ 80 | convert raw pixel level (x,y) coordinate and real depth to real world coordinate 81 | :param raw_coordinate: numpy-array, shape of (time_step, joint_num, 3) 82 | :return: numpy array, real world coordinate, shape of (time_step, joint_num, 3) 83 | """ 84 | real_pose = [] 85 | for poses in raw_coordinate: 86 | real_temp = [] 87 | for x, y, z in poses: 88 | real_x,real_y,real_z = self.cal_real_coordinate(x,y,z) 89 | real_y = round(real_y, 2) 90 | real_x = round(real_x, 2) 91 | real_temp.append(np.array([real_x, real_y, real_z], dtype=np.float32)) 92 | real_pose.append(real_temp) 93 | real_pose = np.array(real_pose) 94 | return real_pose 95 | -------------------------------------------------------------------------------- /kinect_smoothing/depth_image_smoothing.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | import cv2 4 | import numpy as np 5 | import scipy.ndimage.filters as flt 6 | from functools import partial 7 | 8 | 9 | class HoleFilling_Filter(object): 10 | """ 11 | Original Kinect depth image has many invalid pixels (black hole). 12 | This function helps you to fill the invalid pixel with the proper value. 13 | """ 14 | def __init__(self, flag='min',radius=5,min_valid_depth=100, max_valid_depth=4000, 15 | min_valid_neighbors=1,max_radius=20): 16 | """ 17 | :param flag: string, specific methods for hole filling. 18 | 'min': Fill in with the minimum valid value within the neighboring pixels 19 | 'max': Fill in with the maximum valid value within the neighboring pixels 20 | 'mode': Fill in with the mode of valid value within the neighboring pixels 21 | 'mean': Fill in with the mean valid value within the neighboring pixels 22 | 'fmi': Fast Matching Inpainting, refer to 'An Image Inpainting Technique Based on the Fast Marching Method' 23 | 'ns': Fluid Dynamics Inpainting, refer to 'Navier-Stokes, Fluid Dynamics, and Image and Video Inpainting' 24 | :param radius: float, radius of the neighboring area used for fill in a hole 25 | :param min_valid_depth: float, a depth pixel is considered as invalid value, when depth < min_valid_depth 26 | :param max_valid_depth: float, a depth pixel is considered as invalid value, when depth > max_valid_depth 27 | :param min_valid_neighbors: int, if the number of valid neighbors > min_valid_neighbors, 28 | then replace the hole with the proper value calculated by these neighboring valid values. 29 | if not, let radius = radius+1, recollect the neighboring valid value. 30 | :param max_radius: float, maximum radius for the neighboring area 31 | """ 32 | self.flag=flag 33 | self.radius=radius 34 | self.valid_depth_min = min_valid_depth 35 | self.valid_depth_max = max_valid_depth 36 | self.min_valid_neighbors = min_valid_neighbors 37 | self.max_radius = max_radius 38 | if flag=='min': 39 | cal_fn = np.min 40 | elif flag=='max': 41 | cal_fn = np.max 42 | elif flag=='mean': 43 | cal_fn = np.mean 44 | elif flag=='mode': 45 | cal_fn = self._cal_mode 46 | else: 47 | cal_fn=None 48 | self.cal_fn=cal_fn 49 | if flag=='fmi': 50 | inpaint_fn=partial(cv2.inpaint,inpaintRadius=radius,flags=cv2.INPAINT_TELEA) 51 | elif flag=='ns': 52 | inpaint_fn=partial(cv2.inpaint,inpaintRadius=radius,flags=cv2.INPAINT_NS) 53 | else: 54 | inpaint_fn=None 55 | self.inpaint_fn=inpaint_fn 56 | 57 | if flag not in self.all_flags: 58 | raise('invalid flags. Only support:', self.all_flags) 59 | 60 | def _get_neighbors(self,img, x, y, radius, img_h, img_w): 61 | """ 62 | collect the neighboring valid value within the range of (x-radius,x+radius), (y-radius,y+radius) 63 | :param img: numpy-array, 64 | :param x: int 65 | :param y: int 66 | :param radius: int 67 | :param img_h: int, height of image 68 | :param img_w: int height of image 69 | :return: (valid_neighbors,valid_num) 70 | valid_neighbors: list, valid neigboring value 71 | valid_num: int, number of valid_neighbors 72 | """ 73 | valid_neighbors = [] 74 | valid_num = 0 75 | for ii in range(-radius, radius, 1): 76 | xx = x + ii 77 | if xx < 0 or xx >= img_h: 78 | continue 79 | for jj in range(-radius, radius, 1): 80 | yy = y + jj 81 | if yy < 0 or yy >= img_w: 82 | continue 83 | pixel = img[xx, yy] 84 | if pixel: 85 | valid_neighbors.append(pixel) 86 | valid_num += 1 87 | return valid_neighbors, valid_num 88 | 89 | def _cal_mode(self,nums): 90 | """ 91 | calculate the mode 92 | :param nums: list 93 | :return: mode of nums 94 | """ 95 | result=nums[0] 96 | cnt=0 97 | for num in nums: 98 | if cnt==0 or num==result: 99 | cnt+=1 100 | result=num 101 | else: 102 | cnt -=1 103 | return result 104 | 105 | def statistical_smoothing(self,image): 106 | """ 107 | smoothing image with statistical filling method, such as min,max, mode, mean 108 | :param image: numpy-array, 109 | :return: smoothed: numpy-array, smoothed image 110 | """ 111 | smoothed = image.copy() 112 | h, w = image.shape 113 | image[image <= self.valid_depth_min] = 0 114 | image[image >= self.valid_depth_max] = 0 115 | invalid_x, invalid_y = np.where(image == 0) 116 | 117 | for x, y in zip(invalid_x, invalid_y): 118 | _r = self.radius 119 | valid_neighbors,valid_num = [],0 120 | while valid_num < self.min_valid_neighbors and _r < self.max_radius: 121 | valid_neighbors, valid_num = self._get_neighbors(image, x, y,_r, h, w) 122 | _r += 1 123 | if len(valid_neighbors)>0: 124 | smoothed[x, y]= self.cal_fn(valid_neighbors) 125 | 126 | return smoothed 127 | 128 | def inpainting_smoothing(self,image): 129 | """ 130 | smoothing image with inpainting method, such as FMI, NS 131 | :param image: numpy-array, 132 | :return: smoothed: numpy-array, smoothed image 133 | """ 134 | image[image <= self.valid_depth_min] = 0 135 | image[image >= self.valid_depth_max] = 0 136 | mask = np.zeros(image.shape, dtype=np.uint8) 137 | mask[image==0] = 1 138 | smoothed = self.inpaint_fn(image,mask[:,:,np.newaxis]) 139 | smoothed[0]=smoothed[2] # first 2 lines is zero 140 | smoothed[1]=smoothed[2] 141 | return smoothed 142 | 143 | def smooth_image(self,image): 144 | """ 145 | smooth the image using specific method 146 | :param image: numpy-array, 147 | :return: smoothed_image: numpy-array, 148 | """ 149 | image = image.copy() 150 | if self.flag in ['min','max','mean','mode']: 151 | smoothed_image = self.statistical_smoothing(image) 152 | elif self.flag in ['fmi','ns']: 153 | smoothed_image = self.inpainting_smoothing(image) 154 | else: 155 | raise ('invalid smoothing flags') 156 | return smoothed_image 157 | 158 | def smooth_image_frames(self,image_frames): 159 | """ 160 | smooth image frames 161 | :param image_frames: list of numpy array 162 | :return: smoothed_frames: list of numpy array 163 | """ 164 | smoothed_frames=[] 165 | for imgs in image_frames: 166 | if not isinstance(imgs,list): 167 | imgs=[imgs] 168 | for img in imgs: 169 | res_img = self.smooth_image(img) 170 | smoothed_frames.append(res_img) 171 | return smoothed_frames 172 | 173 | @property 174 | def all_flags(self): 175 | flags=[ 176 | 'min', 177 | 'max', 178 | 'mean', 179 | 'mode', 180 | 'fmi', # Fast matching inpainting 181 | 'ns', # NS inpainting 182 | ] 183 | return flags 184 | 185 | 186 | class Denoising_Filter(object): 187 | """ 188 | Denoising filter can be used to improve the resolution of the depth image 189 | """ 190 | def __init__(self, flag='modeling',theta=30, threshold=10,depth_min=100,depth_max=4000, 191 | ksize=5, sigma=0.1,niter=1,kappa=50,gamma=1,option=1): 192 | """ 193 | :param flag: string, specific methods for denoising. 194 | 'modeling': filter with Kinect V2 noise model, 'Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking' 195 | 'modeling_pf': another Kinect V2 noise modeling by Peter Fankhauser, 'Kinect v2 for Mobile Robot Navigation: Evaluation and Modeling' 196 | 'anisotropic': smoothing with anisotropic filtering, 'Scale-space and edge detection using anisotropic diffusion' 197 | 'gaussian': smoothing with Gaussian filtering 198 | :param theta: float, the average angle between Kinect z-axis and the object plane. 199 | Used to calculate noise in the 'modeling' and 'modeling_pf' method 200 | :param threshold: int, thrshold for 'modeling' and 'modeling_pf' method. 201 | :param depth_min: float, minimum valid depth, we only filter the area of depth > depth_min 202 | :param depth_max: float, maximum valid depth, we only filter the area of depth < depth_max 203 | :param ksize: int, Gaussian kernel size 204 | :param sigma: float, Gaussian kernel standard deviation 205 | :param niter: int, number of iterations for anisotropic filtering 206 | :param kappa: int, conduction coefficient for anisotropic filtering, 20-100 ? 207 | :param gamma: float, max value of .25 for stability 208 | :param option: 1 or 2, options for anisotropic filtering 209 | 1: Perona Malik diffusion equation No. 1 210 | 2: Perona Malik diffusion equation No. 2 211 | """ 212 | self.flag=flag 213 | self.f_x = 585 #focal length of the Kinect camera in pixel 214 | if flag=='modeling' or flag=='modeling_pf': 215 | self.threshold = threshold 216 | self.depth_min = depth_min 217 | self.depth_max = depth_max 218 | self.theta = theta 219 | self.noise_type='pf' if flag=='modeling_pf' else 'normal' 220 | self.filter = partial(self.modeling_filter,theta=theta, 221 | threshold=threshold, depth_min=depth_min, 222 | depth_max=depth_max,noise_type=self.noise_type) 223 | elif flag=='gaussian': 224 | self.ksize = ksize 225 | self.sigma = sigma 226 | self.filter = partial(cv2.GaussianBlur,ksize=(ksize,ksize),sigmaX=0) 227 | elif flag=='anisotropic': 228 | self.niter = niter 229 | self.kappa = kappa 230 | self.gamma=gamma 231 | self.sigma=sigma 232 | self.option=option 233 | self.filter = partial(self.anisotropic_filter,niter=niter,kappa=kappa, 234 | gamma=gamma,sigma=sigma) 235 | 236 | if flag not in self.all_flags: 237 | raise('invalid flags. Only support:', self.all_flags) 238 | 239 | 240 | def _axial_noise(self, z, theta): 241 | """ 242 | calculate the axial noise based on 'Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking' 243 | :param z: float, depth 244 | :param theta: float, angle 245 | :return: sigma: float, axial noise 246 | """ 247 | z = z / 1000 248 | theta = math.pi * theta / 180 249 | sigma = 0.0012 + 0.0019 * (z - 0.4) ** 2 + (0.0001 / np.sqrt(z)) * (theta ** 2 / (math.pi / 2 - theta) ** 2) 250 | sigma = sigma * 1000 251 | return sigma 252 | 253 | def _lateral_noise(self, z, theta): 254 | """ 255 | calculate the lateral noise based on 'Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking' 256 | :param z: float, depth 257 | :param theta: float, angle 258 | :return: sigma: float, lateral noise 259 | """ 260 | f_x = 585 # 357 261 | theta = math.pi * theta / 180 262 | sigma_pixel = 0.8 + 0.035 * theta / (math.pi / 2 - theta) 263 | sigma = sigma_pixel * z / f_x 264 | return sigma 265 | 266 | def _axial_noise_pf(self, z, theta): 267 | """ 268 | calculate the axial noise based on 'Kinect v2 for Mobile Robot Navigation: Evaluation and Modeling' 269 | :param z: float, depth 270 | :param theta: float, angle 271 | :return: sigma: float, axial noise 272 | """ 273 | z = z / 1000 274 | theta = math.pi * theta / 180 275 | sigma = 1.5 - 0.5 * z + 0.3 * np.power(z, 2) + 0.1 * np.power(z, 3 / 2) * np.power(theta, 2) / np.power( 276 | math.pi / 2 - theta, 2) 277 | return sigma 278 | 279 | def _lateral_noise_pf(self,z, theta=0, shadow=False): 280 | """ 281 | calculate the lateral noise based on 'Kinect v2 for Mobile Robot Navigation: Evaluation and Modeling' 282 | :param z: float, depth 283 | :param theta: float, angle 284 | :param shadow: bool, 285 | :return: sigma: float, lateral noise 286 | """ 287 | if shadow: 288 | sigma = 3.1 289 | else: 290 | sigma = 1.6 291 | sigma = sigma * np.ones(z.shape) 292 | return sigma 293 | 294 | 295 | def anisotropic_filter(self,img,niter=1,kappa=50,gamma=0.1,step=(1.,1.),sigma=0, option=1): 296 | """ 297 | Anisotropic diffusion. 298 | usage: imgout = anisodiff(im, niter, kappa, gamma, option) 299 | 300 | :param img: - input image 301 | :param niter: - number of iterations 302 | :param kappa: - conduction coefficient 20-100 ? 303 | :param gamma: - max value of .25 for stability 304 | :param step: - tuple, the distance between adjacent pixels in (y,x) 305 | :param option: - 1 Perona Malik diffusion equation No 1 306 | 2 Perona Malik diffusion equation No 2 307 | 308 | :return: imgout - diffused image. 309 | 310 | kappa controls conduction as a function of the gradient. If kappa is low 311 | small intensity gradients are able to block conduction and hence diffusion 312 | across step edges. A large value reduces the influence of intensity 313 | gradients on conduction. 314 | 315 | gamma controls speed of diffusion (you usually want it at a maximum of 316 | 0.25) 317 | 318 | step is used to scale the gradients in case the spacing between adjacent 319 | pixels differs in the x and y axes 320 | 321 | Diffusion equation 1 favours high contrast edges over low contrast ones. 322 | Diffusion equation 2 favours wide regions over smaller ones. 323 | 324 | Reference: 325 | P. Perona and J. Malik. 326 | Scale-space and edge detection using ansotropic diffusion. 327 | IEEE Transactions on Pattern Analysis and Machine Intelligence, 328 | 12(7):629-639, July 1990. 329 | 330 | Original MATLAB code by Peter Kovesi 331 | School of Computer Science & Software Engineering 332 | The University of Western Australia 333 | pk @ csse uwa edu au 334 | 335 | 336 | Translated to Python and optimised by Alistair Muldal 337 | Department of Pharmacology 338 | University of Oxford 339 | 340 | 341 | June 2000 original version. 342 | March 2002 corrected diffusion eqn No 2. 343 | July 2012 translated to Python 344 | """ 345 | 346 | # initialize output array 347 | img = img.astype('float32') 348 | imgout = img.copy() 349 | 350 | # initialize some internal variables 351 | deltaS = np.zeros_like(imgout) 352 | deltaE = deltaS.copy() 353 | NS = deltaS.copy() 354 | EW = deltaS.copy() 355 | gS = np.ones_like(imgout) 356 | gE = gS.copy() 357 | 358 | for ii in np.arange(1,niter): 359 | 360 | # calculate the diffs 361 | deltaS[:-1,: ] = np.diff(imgout,axis=0) 362 | deltaE[: ,:-1] = np.diff(imgout,axis=1) 363 | 364 | if 0 depth_min 402 | :param depth_max: float, maximum valid depth, we only filter the area of depth < depth_max 403 | :param noise_type: 'normal' of 'pf', 404 | 'normal': noise modeling based on 'Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking' 405 | 'pf': noise modeling based on 'Kinect v2 for Mobile Robot Navigation: Evaluation and Modeling' 406 | :return: denoised img: numpy-array 407 | """ 408 | 409 | denoised_img = img.copy() 410 | h, w = img.shape 411 | lateral_noise = self._lateral_noise_pf if noise_type=='pf' else self._lateral_noise 412 | axial_noise = self._axial_noise_pf if noise_type == 'pf' else self._axial_noise 413 | l_noise = np.power(lateral_noise(img, theta), 2) 414 | z_noise = np.power(axial_noise(img, theta), 2) 415 | distance_metrics = np.array([[1.414, 1, 1.414], 416 | [1, 0, 1], 417 | [1.414, 1, 1.414]]) 418 | for x in range(h): 419 | for y in range(w): 420 | D_u = img[x, y] 421 | if D_u >= depth_min and D_u <= depth_max: 422 | sigmal_l = l_noise[x, y] 423 | sigmal_z = z_noise[x, y] 424 | D_k = img[max(x - 1, 0):x + 2, max(y - 1, 0):y + 2] 425 | delta_z = abs(D_k - D_u) 426 | delta_u = distance_metrics[int(x == 0):2 + int(x < h - 1), int(y == 0):2 + int(y < w - 1)] 427 | mark = delta_z < threshold 428 | D_k_list = D_k[mark].flatten() 429 | u_list = delta_u[mark].flatten() 430 | z_list = delta_z[mark].flatten() 431 | if len(D_k_list) > 0: 432 | w_k_list = np.exp(- u_list ** 2 / (2 * sigmal_l) - z_list ** 2 / (2 * sigmal_z)) 433 | denoised_img[x, y] = np.sum(D_k_list * w_k_list) / w_k_list.sum() 434 | return denoised_img 435 | 436 | def smooth_image(self,image): 437 | """ 438 | smooth the image using a specified method 439 | :param image: numpy-array, 440 | :return: smoothed_image: numpy-array, 441 | """ 442 | image=image.copy() 443 | smoothed = self.filter(image) 444 | return smoothed 445 | 446 | def smooth_image_frames(self, image_frames): 447 | """ 448 | smooth image frames 449 | :param image_frames: list of numpy array 450 | :return: smoothed_frames: list of numpy array 451 | """ 452 | smoothed_frames = [] 453 | for imgs in image_frames: 454 | if not isinstance(imgs, list): 455 | imgs = [imgs] 456 | for img in imgs: 457 | res_img = self.smooth_image(img) 458 | smoothed_frames.append(res_img) 459 | return smoothed_frames 460 | 461 | @property 462 | def all_flags(self): 463 | flags = [ 464 | 'modeling', 465 | 'modeling_pf', 466 | 'gaussian', 467 | 'anisotropic', 468 | ] 469 | return flags 470 | -------------------------------------------------------------------------------- /kinect_smoothing/pipeline.py: -------------------------------------------------------------------------------- 1 | 2 | from .depth_image_smoothing import HoleFilling_Filter, Denoising_Filter 3 | from .trajectory_smoothing import Crop_Filter, Smooth_Filter, Motion_Sampler 4 | from .coordinate_calculation import Coordinate_Calculator 5 | 6 | class Kinect_Openpose_Pipeline(object): 7 | """ 8 | simple Kinect-Openpose preprocessing pipeline for trajectory prediction 9 | """ 10 | def __init__(self,image_holefilling=None, image_denoising=None, 11 | tranjectory_crop=None, tranjectory_smooth=None, 12 | motion_sampler=None,coordinate_calculator=None): 13 | """ 14 | :param image_holefilling: class of HoleFilling_Filter 15 | :param image_denoising: class of Denoising_Filter 16 | :param tranjectory_crop: class of Crop_Filter 17 | :param tranjectory_kalman: class of Smooth_Filter 18 | :param motion_sampler: class of Motion_Sampler 19 | :param coordinate_calculator: class of Coordinate_Calculator 20 | """ 21 | self.image_holefilling= image_holefilling if image_holefilling is not None else HoleFilling_Filter() 22 | self.tranjectory_crop = tranjectory_crop if tranjectory_crop is not None else Crop_Filter() 23 | self.coordinate_calculator = coordinate_calculator if coordinate_calculator is not None else Coordinate_Calculator() 24 | self.image_denoising = image_denoising 25 | self.tranjectory_smooth = tranjectory_smooth 26 | self.motion_sampler = motion_sampler 27 | 28 | def __call__(self, image_frame, openpose_frame): 29 | """ 30 | calculate the real world coordinates from kinect-depth images and openpose positions 31 | :param image_frame: list of numpy array, depth-image frames, [img_0,img_1,...], img_i has a shape of (width, height) 32 | :param openpose_frame: list of numpy array, position frames, [pose_0,pose_1,...], pose_i has a shape of (joint_num, 2) 33 | :return: numpy-array, real world coordinates, shape of (time_step, joint_num, 3) 34 | """ 35 | image_frame = self.image_holefilling.smooth_image_frames(image_frame) 36 | 37 | if self.image_denoising is not None: 38 | image_frame = self.image_denoising.smooth_image_frames(image_frame) 39 | 40 | openpose_frame = self.tranjectory_crop.smooth_multi_trajectories(openpose_frame) 41 | 42 | coordinate = self.coordinate_calculator.get_depth_coordinate(image_frame, openpose_frame) 43 | 44 | if self.tranjectory_crop is not None: 45 | coordinate = self.tranjectory_crop.smooth_multi_trajectories(coordinate) 46 | 47 | if self.tranjectory_smooth is not None: 48 | coordinate = self.tranjectory_smooth.smooth_multi_trajectories(coordinate) 49 | 50 | coordinate = self.coordinate_calculator.convert_real_coordinate(coordinate) 51 | 52 | if self.motion_sampler is not None: 53 | coordinate = self.motion_sampler.motion_detection(coordinate) 54 | 55 | return coordinate 56 | -------------------------------------------------------------------------------- /kinect_smoothing/trajectory_smoothing.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from functools import partial 3 | from scipy import interpolate, signal 4 | import pykalman 5 | 6 | class Crop_Filter(object): 7 | """ 8 | The x, y coordinates of the trajectory were captured by some keypoint detection algorithms (e.g. Openpose). 9 | Sometimes objects will be placed in the background and the depth coordinates may register as invalid values. 10 | The Crop-Filter crops the invalid values and runs some interpolation methods to replace them. 11 | """ 12 | def __init__(self,flag='pchip',min_valid_value=100,max_valid_value=1300): 13 | """ 14 | :param flag: string, specifies the method for crop filtering on the data, 15 | such as "zero","linear","slinear","quadratic","cubic","previous","next","nearest". 16 | 'pchip': PCHIP 1-d monotonic cubic interpolation, refer to 'Monotone Piecewise Cubic Interpolation' 17 | 'akima': Akima 1D Interpolator, refer to 'A new method of interpolation and smooth curve fitting based on local procedures' 18 | :param min_valid_value: float, crop-filter the value < min_valid_value 19 | :param max_valid_value: float, crop-filter the value > max_valid_value 20 | """ 21 | self.flag=flag 22 | self.min_value = min_valid_value 23 | self.max_value = max_valid_value 24 | 25 | if flag in ["zero","linear","slinear","quadratic","cubic","previous","next","nearest"]: 26 | self.filter = partial(interpolate.interp1d,kind=flag) 27 | elif flag=='pchip' or flag=='PchipInterpolator': 28 | self.filter = interpolate.PchipInterpolator 29 | elif flag=='akima' or flag=='Akima1DInterpolator': 30 | self.filter = interpolate.Akima1DInterpolator 31 | 32 | if flag not in self.all_flags: 33 | raise('invalid flags. Only support:', self.all_flags) 34 | 35 | def smooth_trajectory_1d(self,trajectory_1d): 36 | """ 37 | smooth the 1-d trajectory or time-series data 38 | :param trajectory_1d: numpy array, shape of (time_step,) 39 | :return: numpy-array, smoothed trajectory, same shape as input trajectory_1d 40 | """ 41 | valid_ind_s = np.where(trajectory_1d >= self.min_value)[0] 42 | valid_ind_l = np.where(trajectory_1d <= self.max_value)[0] 43 | valid_ind = np.intersect1d(valid_ind_s,valid_ind_l,return_indices=False) 44 | if len(valid_ind)==len(trajectory_1d): 45 | return trajectory_1d 46 | 47 | t = np.arange(len(trajectory_1d)) 48 | interp_fn = self.filter(valid_ind,trajectory_1d[valid_ind]) 49 | left_ind,right_ind = valid_ind[0],valid_ind[-1] 50 | smoothed_ind = t[left_ind:right_ind+1] 51 | smoothed_1d = interp_fn(smoothed_ind) # only interpolate middle are, 52 | if left_ind>0: 53 | left_val = trajectory_1d[left_ind]*np.ones(left_ind) 54 | smoothed_1d = np.concatenate([left_val,smoothed_1d]) 55 | if right_ind max_valid_gradient 120 | """ 121 | self.flag=flag 122 | self.max_valid_gradient = max_valid_gradient 123 | 124 | if flag in ["zero","linear","slinear","quadratic","cubic","previous","next","nearest"]: 125 | self.filter = partial(interpolate.interp1d,kind=flag) 126 | elif flag=='pchip' or flag=='PchipInterpolator': 127 | self.filter = interpolate.PchipInterpolator 128 | elif flag=='akima' or flag=='Akima1DInterpolator': 129 | self.filter = interpolate.Akima1DInterpolator 130 | 131 | if flag not in self.all_flags: 132 | raise('invalid flags. Only support:', self.all_flags) 133 | 134 | def smooth_trajectory_1d(self,trajectory_1d): 135 | """ 136 | smooth the 1-d trajectory or time-series data 137 | :param trajectory_1d: numpy array, shape of (time_step,) 138 | :return: numpy-array, smoothed trajectory, same shape as input trajectory_1d 139 | """ 140 | 141 | valid_ind = [] 142 | valid_value = trajectory_1d[0] 143 | for ii, val in enumerate(trajectory_1d): 144 | if abs(valid_value - val) < self.max_valid_gradient : 145 | valid_value = val 146 | valid_ind.append(ii) 147 | elif ii ==len(trajectory_1d)-1: 148 | valid_ind.append(ii) 149 | 150 | if len(valid_ind)==len(trajectory_1d): 151 | return trajectory_1d 152 | 153 | t = np.arange(len(trajectory_1d)) 154 | interp_fn = self.filter(valid_ind,trajectory_1d[valid_ind]) 155 | left_ind,right_ind = valid_ind[0],valid_ind[-1] 156 | smoothed_ind = t[left_ind:right_ind+1] 157 | smoothed_1d = interp_fn(smoothed_ind) # only interpolate middle are, 158 | if left_ind>0: 159 | left_val = trajectory_1d[left_ind]*np.ones(left_ind) 160 | smoothed_1d = np.concatenate([left_val,smoothed_1d]) 161 | if right_ind= thresh or (ii>=left_valid_ind and ii<=right_valid_ind): 433 | motion_data.append(trajectory[ii]) 434 | if len(motion_data) < self.min_time_step and thresh<=0: 435 | motion_data = self.motion_detection(trajectory, thresh // 2) 436 | motion_data = np.array(motion_data) 437 | return motion_data 438 | 439 | def interpolate_trajectory(self,trajectory): 440 | """ 441 | interpolate and upsample the trajectory 442 | :param trajectory: numpy-array, shape of (time_step, coordinate_dim) 443 | :return: interpolated trajectory, which has a shape of (interpolate_ratio*time_step, coordinate_dim) 444 | """ 445 | if len(trajectory.shape)<2: 446 | trajectory=np.expand_dims(trajectory, axis=1) 447 | L,feat_dim =trajectory.shape[:2] 448 | t=np.arange(L) 449 | 450 | ratio = self.interpolate_ratio[0] 451 | for rat, thr in zip (self.interpolate_ratio[1:],self.interpolate_threshold): 452 | if L>thr: 453 | ratio=rat 454 | if ratio<=1: 455 | return trajectory 456 | new_t = [ii/ratio for ii in list(range(ratio*(L-1)+1))] 457 | interp_data=np.zeros((len(new_t),feat_dim)) 458 | for jj in range(feat_dim): 459 | f = self.interpolator(t, trajectory[:,jj]) 460 | new_traj = f(new_t) 461 | interp_data[:,jj] = new_traj 462 | 463 | return interp_data 464 | 465 | def interpolate_multi_trajectories(self,trajectories): 466 | """ 467 | interpolate and upsample the multi-joint-trajectory 468 | :param trajectory: numpy-array, shape of (time_step, joint_num, coordinate_dim) 469 | :return: interpolated trajectory, which has a shape of (interpolate_ratio*time_step, joint_num, coordinate_dim) 470 | """ 471 | if len(trajectories.shape)<3: 472 | trajectories=np.expand_dims(trajectories, axis=1) 473 | 474 | L,joint_num, feat_dim = trajectories.shape[:3] 475 | ratio = self.interpolate_ratio[0] 476 | for rat, thr in zip(self.interpolate_ratio[1:],self.interpolate_threshold): 477 | if L>thr: 478 | ratio=rat 479 | new_t = [ii/ratio for ii in list(range(ratio*(L-1)+1))] 480 | multi_interpolated=np.zeros((len(new_t),joint_num,feat_dim)) 481 | for ii in range(joint_num): 482 | multi_interpolated[:, ii] = self.interpolate_trajectory(trajectories[:, ii]) 483 | return multi_interpolated 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | -------------------------------------------------------------------------------- /kinect_smoothing/utils.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from mpl_toolkits.mplot3d import Axes3D 4 | 5 | 6 | def plot_image_frame(image_frame): 7 | """ 8 | utils for plot image frames 9 | :param image_frame: list of images 10 | """ 11 | for ii, image in enumerate(image_frame): 12 | plt.figure() 13 | if isinstance(image, list): 14 | image = image[0] 15 | plt.imshow(image) 16 | plt.title('frame: ' + str(ii)) 17 | plt.show() 18 | 19 | 20 | def plot_trajectories(pose_frame): 21 | """ 22 | utils for plot trajectory related to time-step t 23 | :param pose_frame: numpy-array, (time_step,joint_num, ccordinate_dim) 24 | """ 25 | pose_frame = np.array(pose_frame) 26 | timestep, joint_num, dim = pose_frame.shape 27 | joints = ['neck', 'shoulder', 'elbow', 'hand'] 28 | plt.figure(figsize=(12, 7)) 29 | t = np.arange(timestep) 30 | for ii, mark in enumerate(joints): 31 | plt.subplot(331) 32 | plt.plot(t, pose_frame[:, ii, 0], label=mark) 33 | plt.xlabel('t') 34 | plt.ylabel('x') 35 | plt.subplot(332) 36 | plt.plot(t, pose_frame[:, ii, 1], label=mark) 37 | plt.xlabel('t') 38 | plt.ylabel('y') 39 | if dim > 2: 40 | plt.subplot(333) 41 | plt.plot(t, pose_frame[:, ii, 2], label=mark) 42 | plt.xlabel('t') 43 | plt.ylabel('z') 44 | plt.subplots_adjust(wspace=0.5, hspace=0) 45 | plt.legend(loc=(1, 0.4)) 46 | plt.show() 47 | 48 | 49 | def plot_trajectory_3d(trajectory): 50 | """ 51 | plot 3d trajectory 52 | :param trajectory: numpy-array, shape of (time_step,3) 53 | """ 54 | xs = trajectory[:, 0] 55 | ys = trajectory[:, 1] 56 | zs = trajectory[:, 2] 57 | fig = plt.figure() 58 | ax = Axes3D(fig) 59 | ax.plot3D(xs, ys, zs=zs, marker='o', color='b') 60 | plt.show() 61 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | opencv-python 4 | pykalman --------------------------------------------------------------------------------