diff --git a/.gitignore b/.gitignore index 67f2384..c868f39 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ install log .git venv +.vscode +.idea + diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 0e6c965..c17a30a 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -8,6 +8,10 @@ "includePath": [ "${workspaceFoler}/**", "${workspaceFolder}/install/custom_interfaces/include/custom_interfaces", + "${workspaceFolder}/src/custom_interfaces/include/custom_interfaces", + "${workspaceFolder}/src/custom_interfaces/include/custom_interfaces/**", + "${workspaceFolder}/src/custom_interfaces/include/custom_interfaces/msg", + "${workspaceFolder}/src/custom_interfaces/msg", "/opt/ros/humble/include/**", "/home/daeyun/Desktop/github_repos/SIFM/**", "/home/daeyun/Desktop/github_repos/SIFM/src/tcp_pkg/include", diff --git a/common/RealSense_Utilities.zip b/common/RealSense_Utilities.zip new file mode 100644 index 0000000..d622e47 Binary files /dev/null and b/common/RealSense_Utilities.zip differ diff --git a/common/RealSense_Utilities/MATLAB/depth_options.txt b/common/RealSense_Utilities/MATLAB/depth_options.txt new file mode 100644 index 0000000..9b3c7a0 --- /dev/null +++ b/common/RealSense_Utilities/MATLAB/depth_options.txt @@ -0,0 +1,53 @@ +/device_0/sensor_0/option/Alternate_IR/description, Enable/Disable alternate IR +/device_0/sensor_0/option/Alternate_IR/value, 0.000000 +/device_0/sensor_0/option/Apd_Temperature/description, Avalanche Photo Diode temperature +/device_0/sensor_0/option/Apd_Temperature/value, 33.078812 +/device_0/sensor_0/option/Confidence_Threshold/description, The confidence level threshold to use to mark a pixel as valid by the depth algorithm +/device_0/sensor_0/option/Confidence_Threshold/value, 1.000000 +/device_0/sensor_0/option/Depth_Offset/description, Offset from sensor to depth origin in millimetrers +/device_0/sensor_0/option/Depth_Offset/value, 4.500000 +/device_0/sensor_0/option/Depth_Units/description, Number of meters represented by a single depth unit +/device_0/sensor_0/option/Depth_Units/value, 0.000250 +/device_0/sensor_0/option/Digital_Gain/description, Change the depth digital gain to: 1 for high gain and 2 for low gain +/device_0/sensor_0/option/Digital_Gain/value, 1.000000 +/device_0/sensor_0/option/Error_Polling_Enabled/description, Enable / disable polling of camera internal errors +/device_0/sensor_0/option/Error_Polling_Enabled/value, 1.000000 +/device_0/sensor_0/option/Frames_Queue_Size/description, Max number of frames you can hold at a given time. Increasing this number will reduce frame drops but increase latency, and vice versa +/device_0/sensor_0/option/Frames_Queue_Size/value, 16.000000 +/device_0/sensor_0/option/Freefall_Detection_Enabled/description, When enabled (default), the sensor will turn off if a free-fall is detected +/device_0/sensor_0/option/Freefall_Detection_Enabled/value, 1.000000 +/device_0/sensor_0/option/Global_Time_Enabled/description, Enable/Disable global timestamp +/device_0/sensor_0/option/Global_Time_Enabled/value, 0.000000 +/device_0/sensor_0/option/Host_Performance/description, Optimize based on host performance, low power low performance host or high power high performance host +/device_0/sensor_0/option/Host_Performance/value, 0.000000 +/device_0/sensor_0/option/Humidity_Temperature/description, Humidity temperature +/device_0/sensor_0/option/Humidity_Temperature/value, 35.964371 +/device_0/sensor_0/option/Inter_Cam_Sync_Mode/description, Enable multi-camera hardware synchronization mode (disabled on startup); not compatible with free-fall detection +/device_0/sensor_0/option/Inter_Cam_Sync_Mode/value, 0.000000 +/device_0/sensor_0/option/Invalidation_Bypass/description, Enable/disable pixel invalidation +/device_0/sensor_0/option/Invalidation_Bypass/value, 0.000000 +/device_0/sensor_0/option/LDD_temperature/description, Laser Driver temperature +/device_0/sensor_0/option/LDD_temperature/value, 41.458199 +/device_0/sensor_0/option/Laser_Power/description, Power of the laser emitter, with 0 meaning projector off +/device_0/sensor_0/option/Laser_Power/value, 100.000000 +/device_0/sensor_0/option/Ma_Temperature/description, DSP controller temperature +/device_0/sensor_0/option/Ma_Temperature/value, 35.222988 +/device_0/sensor_0/option/Mc_Temperature/description, Mems Controller temperature +/device_0/sensor_0/option/Mc_Temperature/value, 34.228119 +/device_0/sensor_0/option/Min_Distance/description, Minimal distance to the target (in mm) +/device_0/sensor_0/option/Min_Distance/value, 490.000000 +/device_0/sensor_0/option/Noise_Estimation/description, Noise estimation +/device_0/sensor_0/option/Noise_Estimation/value, 1242.000000 +/device_0/sensor_0/option/Noise_Filtering/description, Control edges and background noise +/device_0/sensor_0/option/Noise_Filtering/value, 4.000000 +/device_0/sensor_0/option/Post_Processing_Sharpening/description, Changes the amount of sharpening in the post-processed image +/device_0/sensor_0/option/Post_Processing_Sharpening/value, 1.000000 +/device_0/sensor_0/option/Pre_Processing_Sharpening/description, Changes the amount of sharpening in the pre-processed image +/device_0/sensor_0/option/Pre_Processing_Sharpening/value, 0.000000 +/device_0/sensor_0/option/Receiver_Gain/description, Changes the exposure time of Avalanche Photo Diode in the receiver +/device_0/sensor_0/option/Receiver_Gain/value, 9.000000 +/device_0/sensor_0/option/Sensor_Mode/description, Notify the sensor about the intended streaming mode. Required for preset +/device_0/sensor_0/option/Sensor_Mode/value, 2.000000 +/device_0/sensor_0/option/Visual_Preset/description, Preset to calibrate the camera to environment ambient, no ambient or low ambient. +/device_0/sensor_0/option/Visual_Preset/value, 4.000000 +/device_0/sensor_0/post_processing, Temporal Filter diff --git a/common/RealSense_Utilities/MATLAB/get_options_from_rosbag.m b/common/RealSense_Utilities/MATLAB/get_options_from_rosbag.m new file mode 100644 index 0000000..11182dd --- /dev/null +++ b/common/RealSense_Utilities/MATLAB/get_options_from_rosbag.m @@ -0,0 +1,40 @@ +bag = rosbag("C:\Users\35840\Documents\20211217_204044.bag"); +topics = bag.AvailableTopics; + +% Get only the option topics +depth_sensor_option_topics = topics(14:66,:).Properties.RowNames; +rgb_sensor_option_topics = topics(73:105,:).Properties.RowNames; +imu_sensor_option_topics = topics(117:122,:).Properties.RowNames; + +% Put options data in cells +depth_options = options_to_cell(bag, depth_sensor_option_topics) +rgb_options = options_to_cell(bag, rgb_sensor_option_topics) +imu_options = options_to_cell(bag, imu_sensor_option_topics) + +cell_to_file(depth_options, 'depth_options.txt') +cell_to_file(rgb_options, 'rgb_options.txt') +cell_to_file(imu_options, 'imu_options.txt') +function cell_to_file(cell, file_name) + file = fopen(file_name,'w') + for index = 1 : numel(cell) / 2 + disp(index) + if mod(index,2) == 0 + fprintf(file,'%s, %f\n',cell{index,1},cell{index,2}); + else + fprintf(file,'%s, %s\n',cell{index,1},cell{index,2}); + end + end + fclose(file); +end + +function options = options_to_cell(bag, topics) + for index = 1 : numel(topics) + m = readMessages(select(bag,'Topic',topics{index}),'DataFormat','struct'); + options{index,1} = topics{index}; + if mod(index,2) == 0 + options{index,2} = m{1,1}.Data; + else + options{index,2} = m{1,1}.Data; + end + end +end \ No newline at end of file diff --git a/common/RealSense_Utilities/MATLAB/imu_options.txt b/common/RealSense_Utilities/MATLAB/imu_options.txt new file mode 100644 index 0000000..81a93f1 --- /dev/null +++ b/common/RealSense_Utilities/MATLAB/imu_options.txt @@ -0,0 +1,6 @@ +/device_0/sensor_2/option/Enable_Motion_Correction/description, Enable/Disable Automatic Motion Data Correction +/device_0/sensor_2/option/Enable_Motion_Correction/value, 1.000000 +/device_0/sensor_2/option/Frames_Queue_Size/description, Max number of frames you can hold at a given time. Increasing this number will reduce frame drops but increase latency, and vice versa +/device_0/sensor_2/option/Frames_Queue_Size/value, 16.000000 +/device_0/sensor_2/option/Global_Time_Enabled/description, Enable/Disable global timestamp +/device_0/sensor_2/option/Global_Time_Enabled/value, 0.000000 diff --git a/common/RealSense_Utilities/MATLAB/rgb_options.txt b/common/RealSense_Utilities/MATLAB/rgb_options.txt new file mode 100644 index 0000000..88f1a50 --- /dev/null +++ b/common/RealSense_Utilities/MATLAB/rgb_options.txt @@ -0,0 +1,33 @@ +/device_0/sensor_1/option/Auto_Exposure_Priority/description, Restrict Auto-Exposure to enforce constant FPS rate. Turn ON to remove the restrictions (may result in FPS drop) +/device_0/sensor_1/option/Auto_Exposure_Priority/value, 1.000000 +/device_0/sensor_1/option/Backlight_Compensation/description, Enable / disable backlight compensation +/device_0/sensor_1/option/Backlight_Compensation/value, 128.000000 +/device_0/sensor_1/option/Brightness/description, UVC image brightness +/device_0/sensor_1/option/Brightness/value, 0.000000 +/device_0/sensor_1/option/Contrast/description, UVC image contrast +/device_0/sensor_1/option/Contrast/value, 50.000000 +/device_0/sensor_1/option/Enable_Auto_Exposure/description, Enable / disable auto-exposure +/device_0/sensor_1/option/Enable_Auto_Exposure/value, 1.000000 +/device_0/sensor_1/option/Enable_Auto_White_Balance/description, Enable / disable auto-white-balance +/device_0/sensor_1/option/Enable_Auto_White_Balance/value, 1.000000 +/device_0/sensor_1/option/Exposure/description, Controls exposure time of color camera. Setting any value will disable auto exposure +/device_0/sensor_1/option/Exposure/value, 156.000000 +/device_0/sensor_1/option/Frames_Queue_Size/description, Max number of frames you can hold at a given time. Increasing this number will reduce frame drops but increase latency, and vice versa +/device_0/sensor_1/option/Frames_Queue_Size/value, 16.000000 +/device_0/sensor_1/option/Gain/description, UVC image gain +/device_0/sensor_1/option/Gain/value, 256.000000 +/device_0/sensor_1/option/Global_Time_Enabled/description, Enable/Disable global timestamp +/device_0/sensor_1/option/Global_Time_Enabled/value, 0.000000 +/device_0/sensor_1/option/Host_Performance/description, Optimize based on host performance, low power low performance host or high power high performance host +/device_0/sensor_1/option/Host_Performance/value, 0.000000 +/device_0/sensor_1/option/Hue/description, UVC image hue +/device_0/sensor_1/option/Hue/value, 0.000000 +/device_0/sensor_1/option/Power_Line_Frequency/description, Power Line Frequency +/device_0/sensor_1/option/Power_Line_Frequency/value, 1.000000 +/device_0/sensor_1/option/Saturation/description, UVC image saturation setting +/device_0/sensor_1/option/Saturation/value, 50.000000 +/device_0/sensor_1/option/Sharpness/description, UVC image sharpness setting +/device_0/sensor_1/option/Sharpness/value, 50.000000 +/device_0/sensor_1/option/White_Balance/description, Controls white balance of color image. Setting any value will disable auto white balance +/device_0/sensor_1/option/White_Balance/value, 4600.000000 +/device_0/sensor_1/post_processing, Decimation Filter diff --git a/common/RealSense_Utilities/README.md b/common/RealSense_Utilities/README.md new file mode 100644 index 0000000..0e2b188 --- /dev/null +++ b/common/RealSense_Utilities/README.md @@ -0,0 +1 @@ +# RealSense_Utilities diff --git a/common/RealSense_Utilities/object_detection.py b/common/RealSense_Utilities/object_detection.py new file mode 100644 index 0000000..f6afe29 --- /dev/null +++ b/common/RealSense_Utilities/object_detection.py @@ -0,0 +1,113 @@ +'''Program is using this code with modifications: +https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/distance_to_object.ipynb''' +# TODO: +# - +# - +# - +# - + + +import cv2 + +# Color constant for opencv +WHITE = (255, 255, 255) + +class ObjectDetect: + + def __init__(self, color_frame, depth_frame, depth_scale): + self.color_frame = color_frame + self.depth_frame = depth_frame + self.depth_scale = depth_scale + #self.color_intrinsics = color_intrin + + self.confidence: float + + # Rectangle that is drawn around the detected object + self.rectangle_xmin = None + self.rectangle_ymin = None + self.rectangle_xmax = None + self.rectangle_ymax = None + + + def detect(self): + # Standard OpenCV boilerplate for running the net: + height, width = self.color_frame.shape[:2] + expected = 300 + aspect = width / height + resized_image = cv2.resize(self.color_frame, (round(expected * aspect), expected)) + crop_start = round(expected * (aspect - 1) / 2) + crop_img = resized_image[0:expected, crop_start:crop_start+expected] + + net = cv2.dnn.readNetFromCaffe("C:\\Users\\35840\\Downloads\\MobileNetSSD\\MobileNetSSD\\MobileNetSSD_deploy.prototxt", "C:\\Users\\35840\\Downloads\\MobileNetSSD\\MobileNetSSD\\MobileNetSSD_deploy.caffemodel") + inScaleFactor = 0.007843 + meanVal = 127.53 + class_names = ("background", "aeroplane", "bicycle", "bird", "boat", + "bottle", "bus", "car", "cat", "chair", + "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", + "sheep", "sofa", "train", "tvmonitor") + #class_names = ("bottle", "chair", "diningtable", "person", "pottedplant", "tvmonitor") + blob = cv2.dnn.blobFromImage(crop_img, inScaleFactor, (expected, expected), meanVal, False) + net.setInput(blob, "data") + detections = net.forward("detection_out") + + label = detections[0,0,0,1] + conf = detections[0,0,0,2] + xmin = detections[0,0,0,3] + ymin = detections[0,0,0,4] + xmax = detections[0,0,0,5] + ymax = detections[0,0,0,6] + + class_name = class_names[int(label)] + + confidence = str(round(conf,2))[0:4] + + # cv2.rectangle(crop_img, (int(xmin * expected), int(ymin * expected)), + # (int(xmax * expected), int(ymax * expected)), (255, 255, 255), 2) + + scale = height / expected + xmin_depth = int((xmin * expected + crop_start) * scale) + ymin_depth = int((ymin * expected) * scale) + xmax_depth = int((xmax * expected + crop_start) * scale) + ymax_depth = int((ymax * expected) * scale) + + # Crop depth data: + depth = self.depth_frame[xmin_depth:xmax_depth,ymin_depth:ymax_depth].astype(float) + + # Get data scale from the device and convert to meters + depth = depth * self.depth_scale + distance,_,_,_ = cv2.mean(depth) + + # if class_name not in ["bottle", "chair", "diningtable", "person", "pottedplant", "tvmonitor"]: + # class_name = "unknown" + + self.rectangle_xmin = xmin_depth + self.rectangle_ymin = ymin_depth + self.rectangle_xmax = xmax_depth + self.rectangle_ymax = ymax_depth + self.confidence = confidence + self.class_name = class_name + self.distance = distance + + def draw_rectangle(self, image): + + # Rectangle variables + top_left_corner = (self.rectangle_xmin, self.rectangle_ymin) + bottom_right_corner = (self.rectangle_xmax, self.rectangle_ymax) + rectangle_color = WHITE + rectangle_thickness = 2 + + # Text variables + text = f'{self.class_name} {self.distance:.2f} meters away' + text_location = (self.rectangle_xmin, self.rectangle_ymin - 5) + text_font = cv2.FONT_HERSHEY_COMPLEX + text_color = WHITE + text_scale = 0.5 + + cv2.rectangle(image, + top_left_corner, bottom_right_corner, + rectangle_color, rectangle_thickness) + + cv2.putText(self.color_frame, text, + text_location, text_font, + text_scale, text_color) \ No newline at end of file diff --git a/common/RealSense_Utilities/opencv_stream.py b/common/RealSense_Utilities/opencv_stream.py new file mode 100644 index 0000000..bb5a2f5 --- /dev/null +++ b/common/RealSense_Utilities/opencv_stream.py @@ -0,0 +1,179 @@ +import cv2 +from object_detection import ObjectDetect +import math +import numpy as np + +from realsense_api import RealSenseCamera + +top = 100 +bottom = 100 +left = 100 +right = 100 + +BLUE = [255,0,0] + +ros_bag = "C:\\Users\\35840\\Documents\\20211217_204044.bag" + +def measure_dimensions(points): + + height = {'ix': points[0], + 'iy': points[2], + 'x' : points[0], + 'y' : points[3]} + + width = {'ix': points[0], + 'iy': points[2], + 'x' : points[1], + 'y' : points[2]} + + #dimensions = [height, width] + + #print(points) + global color_intrin + + # Height + udist_height = camera.depth_frame.get_distance(height['ix'],height['iy']) + vdist_height = camera.depth_frame.get_distance(height['x'], height['y']) + #print(udist,vdist) + + point1_height = rs.rs2_deproject_pixel_to_point(color_intrin, [height['ix'],height['iy']], udist_height) + point2_height = rs.rs2_deproject_pixel_to_point(color_intrin, [height['x'], height['y']], vdist_height) + #print(str(point1)+ ' ' +str(point2)) + + height = math.sqrt( + math.pow(point1_height[0] - point2_height[0], 2) + math.pow(point1_height[1] - point2_height[1],2) + + math.pow(point1_height[2] - point2_height[2], 2) + ) + + # Width + udist_width = camera.depth_frame.get_distance(width['ix'],width['iy']) + vdist_width = camera.depth_frame.get_distance(width['x'], width['y']) + #print(udist,vdist) + + point1_width = rs.rs2_deproject_pixel_to_point(color_intrin, [width['ix'],width['iy']], udist_width) + point2_width = rs.rs2_deproject_pixel_to_point(color_intrin, [width['x'], width['y']], vdist_width) + #print(str(point1)+ ' ' +str(point2)) + + width = math.sqrt( + math.pow(point1_width[0] - point2_width[0], 2) + math.pow(point1_width[1] - point2_width[1],2) + + math.pow(point1_width[2] - point2_width[2], 2) + ) + + return height, width + +# Initialize the camera +camera = RealSenseCamera(ros_bag) + +display_dimensions = False +show_filtered_distance = False +show_filtered = False +apply_filter = True + +try: + while True: + # try: + camera.get_data() # Load the object's variables with data + depth_frame = camera.depth_frame + color_frame = camera.color_frame + infrared_frame = camera.infrared_frame + color_intrin = camera.color_intrinsics + + # apply filtering to depth data + if apply_filter: + camera.filter_depth_data(enable_decimation = True, + enable_spatial = False, + enable_temporal = True, + enable_hole_filling = False) + + depth_frame = camera.processed_depth_frame + print('filters applied') + #processed_depth_image = camera.frame_to_np_array(processed_depth_frame, colorize_depth=True) + + depth_image = frame_to_np_array(depth_frame, colorize_depth=True) + color_image = frame_to_np_array(color_frame) + infrared_image = frame_to_np_array(infrared_frame) + + ########################################### + ##---------- IMAGE TO BE SHOWN ----------## + ########################################### + image_to_be_shown = depth_image + image_name = 'filtered depth' + + # cv2.imshow('color 640x480', color_image) + # cv2.imshow('depth 320x240', depth_image) + # cv2.imshow('IR 320x240', infrared_image) + a = cv2.resize(image_to_be_shown, (640, 480)) + cv2.imshow(image_name, a) + + + # size = cv2.getWindowImageRect('color 640x480') + # print(size) + + # ret, depth_frame, color_frame, colorized_depth, color_intrin, filtered_depth_colored, filtered_depth = camera.get_frame() + # nn = ObjectDetect(color_frame, colorized_depth, depth_frame, camera.depth_scale, filtered_depth_colored, filtered_depth) + #crop_img, depth_img, text, text_filt, text_location, points, confidence = nn.detect() + + #cv2.namedWindow('Both Streams', cv2.WINDOW_NORMAL) + #cv2.setWindowProperty('Both Strams', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN) + #bordered = cv2.copyMakeBorder(stretched, top, bottom, left, right, cv2.BORDER_CONSTANT, value=BLUE) + + # if show_filtered_distance: + # cv2.putText(color_frame, text_filt, text_location, + # cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255)) + # else: + # print(confidence) + # cv2.putText(color_frame, text, text_location, + # cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255)) + # cv2.putText(colorized_depth, text, text_location, + # cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255)) + + # if display_dimensions: + # height, width = measure_dimensions(points) + # print(height, width) + + # if height != 0.0 or width != 0.0: + # width = f'width = {str(round(width, 3))} meters' + # height = f'height = {str(round(height, 3))} meters' + # text_height_location = (points[1], (int((points[3] + points[2]) / 2))) + # text_width_location = (points[0], points[3] + 20) + # cv2.putText(color_frame, height, text_height_location, + # cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255)) + # cv2.putText(color_frame, width, text_width_location, + # cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255)) + + #images = np.hstack((color_frame, colorized_depth)) + #images_filtered = np.hstack((color_frame, filtered_depth_colored)) + #stretched = cv2.resize(images, (2560, 1440)) + + #cv2.imshow("Filtered", images_filtered) + + if show_filtered: + cv2.imshow('Filtered', filtered_depth_colored) + key = cv2.waitKey(1) + + # Press esc or 'q' to close the image window + if key & 0xFF == ord('q') or key == 27: + cv2.destroyAllWindows() + break + elif key & 0xFF == ord('m'): #enable dimensions measurements + display_dimensions = True + elif key & 0xFF == ord('o'): #turn off dimensions measurements + display_dimensions = False + elif key & 0xFF == ord('f'): + show_filtered_distance = True + elif key & 0xFF == ord('g'): + show_filtered_distance = False + elif key & 0xFF == ord('a'): + show_filtered = True + elif key & 0xFF == ord('s'): + show_filtered = False + cv2.destroyWindow('Filtered') + # elif key & 0xFF == ord('v'): + # camera.spatial_magnitude = + + # except RuntimeError as e: + # print(f'{(e).__class__.__name__}: {e}') #error message + # continue + +finally: + camera.stop() \ No newline at end of file diff --git a/common/RealSense_Utilities/opencv_stream_test.py b/common/RealSense_Utilities/opencv_stream_test.py new file mode 100644 index 0000000..a57f411 --- /dev/null +++ b/common/RealSense_Utilities/opencv_stream_test.py @@ -0,0 +1,59 @@ +import numpy as np # fundamental package for scientific computing +import matplotlib.pyplot as plt # 2D plotting library producing publication quality figures +import pyrealsense2 as rs # Intel RealSense cross-platform open-source API +print("Environment Ready") +import cv2 + +# Setup: +pipe = rs.pipeline() +cfg = rs.config() +cfg.enable_device_from_file("C:\\Users\\35840\\Documents\\20211217_204044.bag") +profile = pipe.start(cfg) + +try: + while True: + # decimate = rs.decimation_filter(8) + # align_to = rs.stream.color + # align = rs.align(align_to) + + # # Get frameset of color and depth + # frames = pipeline.wait_for_frames() + # decimated = decimate.process(frames).as_frameset() + # # Align the depth frame to color frame + # aligned_frames = align.process(decimated) + frameset = pipe.wait_for_frames() + frame = frameset.get_depth_frame() + + depth_to_disparity = rs.disparity_transform(True) + disparity_to_depth = rs.disparity_transform(False) + + colorizer = rs.colorizer() + spatial = rs.spatial_filter() + spatial.set_option(rs.option.holes_fill, 3) + + temporal = rs.temporal_filter(0.4,40,8) + hole_filling = rs.hole_filling_filter() + + # frame = depth_to_disparity.process(frame) + # frame = spatial.process(frame) + frame = temporal.process(frameset).as_frameset() + # frame = frame.get_depth_frame() + + # frame = disparity_to_depth.process(frame) + # frame = hole_filling.process(frame) + align = rs.align(rs.stream.color) + frame = align.process(frame) + frame = frame.get_depth_frame() + colorized_depth = np.asanyarray(colorizer.colorize(frame).get_data()) + + cv2.imshow('o', colorized_depth) + + key = cv2.waitKey(1) + + # Press esc or 'q' to close the image window + if key & 0xFF == ord('q') or key == 27: + cv2.destroyAllWindows() + break + +finally: + pipe.stop() diff --git a/common/RealSense_Utilities/opencv_stream_v2.py b/common/RealSense_Utilities/opencv_stream_v2.py new file mode 100644 index 0000000..701e100 --- /dev/null +++ b/common/RealSense_Utilities/opencv_stream_v2.py @@ -0,0 +1,99 @@ +from object_detection import ObjectDetect +import cv2 +from realsense_api import RealSenseCamera + +# TODO: +# - Restore the object detection functionality +# - Create a dictionary to hold multiple rosbag file paths that can be the +# key while the value can be a camera instantiation. This would be for +# when we want to view multiple rosbags at the same time. +# - Add the ability to change filter values using key presses while viewing +# the opencv Stream +# - Add the ability to save the current filter configuration to a text file +# or json + +#ros_bag = "C:\\Users\\35840\\Documents\\20211217_204044.bag" +# ros_bag = "C:\\Users\\35840\\Documents\\20211220_124541.bag" +ros_bag = "C:\\Users\\35840\\Documents\\20211220_132508.bag" +ros_bag2 = "C:\\Users\\35840\\Documents\\20211221_120843_2d.bag" +ros_bag3 = "C:\\Users\\35840\\Documents\\20211221_121055_3d.bag" + +# Initialize the camera +# camera = RealSenseCamera(ros_bag2) +# camera2 = RealSenseCamera(ros_bag3) +camera = RealSenseCamera() + + +apply_filter = True + +try: + while True: + camera.get_data() # Load the object's variables with data + # camera2.get_data() # Load the object's variables with data + + # apply filtering to depth data + if apply_filter: + camera.filter_depth_data(enable_decimation=True, + enable_spatial=True, + enable_temporal=True, + enable_hole_filling=True) + filtered_frameset = camera.filtered_frameset + + # camera2.filter_depth_data(enable_decimation=True, + # enable_spatial=True, + # enable_temporal=True, + # enable_hole_filling=False) + # filtered_depth_frame2 = camera2.filtered_frameset.get_depth_frame() + + camera.get_aligned_frames(filtered_frameset, aligned_to_color=True) + + #depth_frame = camera.depth_frame + filtered_depth_frame = camera.depth_frame_aligned + + color_frame = camera.color_frame_aligned + # infrared_frame = camera.infrared_frame + # color_intrin = camera.color_intrinsics + + # depth_frame = camera.depth_frame + # color_frame = camera.color_frame + # infrared_frame = camera.infrared_frame + # color_intrin = camera.color_intrinsics + + # depth_frame2 = camera2.depth_frame + # color_frame2 = camera2.color_frame + # infrared_frame2 = camera2.infrared_frame + # color_intrin2 = camera2.color_intrinsics + + color_img = frame_to_np_array(color_frame) + colored_depth_image = frame_to_np_array(filtered_depth_frame, colorize_depth=True) + depth_image = frame_to_np_array(filtered_depth_frame) + + #depth_image = camera.frame_to_np_array(depth_frame, colorize_depth=True) + + detector = ObjectDetect(color_img, depth_image, camera.depth_scale) + detector.detect() + detector.draw_rectangle(color_img) + + image_name = 'filtered depth' + # proc_depth_image2 = camera2.frame_to_np_array(filtered_depth_frame2, colorize_depth=True) + + # depth_image2 = camera2.frame_to_np_array(depth_frame2, colorize_depth=True) + + # img2 = proc_depth_image2 + # image_name2 = 'filtered depth2' + #img = cv2.resize(image_to_be_shown, (640, 480)) + #depth_image = cv2.resize(depth_image, (640, 480)) + + cv2.imshow(image_name, colored_depth_image) + #cv2.imshow('o', depth_image) + # cv2.imshow(image_name2, img2) + cv2.imshow('o2', color_img) + key = cv2.waitKey(1) + + # Press esc or 'q' to close the image window + if key & 0xFF == ord('q') or key == 27: + cv2.destroyAllWindows() + break + +finally: + camera.stop() \ No newline at end of file diff --git a/common/RealSense_Utilities/opencv_stream_v3.py b/common/RealSense_Utilities/opencv_stream_v3.py new file mode 100644 index 0000000..adf8894 --- /dev/null +++ b/common/RealSense_Utilities/opencv_stream_v3.py @@ -0,0 +1,80 @@ +import cv2 +import pyrealsense2 as rs +from RealSense_Utilities.realsense_api.realsense_api import RealSenseCamera +from RealSense_Utilities.realsense_api.realsense_api import find_realsense +from RealSense_Utilities.realsense_api.realsense_api import frame_to_np_array +from RealSense_Utilities.realsense_api.post_processing.option import OptionType + +# TODO: +# - Create a dictionary to hold multiple rosbag file paths that can be the +# key while the value can be a camera instantiation. This would be for +# when we want to view multiple rosbags at the same time. +# - Add the ability to change filter values using key presses while viewing +# the opencv Stream +# - Add the ability to save the current filter configuration to a text file +# or json + + +# Initialize the camera +cameras = [] +realsense_device = find_realsense() + +for i in realsense_device: + cameras.append(RealSenseCamera(device=i, adv_mode_flag=True)) + +apply_filter = False +apply_align = True + +try: + while True: + # Get the frameset and other data to be loaded into the class attributes + for i in range(len(cameras)): + cameras[i].get_data() + + if apply_filter: + cameras[i].filter_depth_data(enable_decimation=False, + enable_spatial=True, + enable_temporal=True, + enable_hole_filling=False) + frameset = cameras[i].filtered_frameset + else: + frameset = cameras[i].frameset + + # filtering block + # frameset = cameras[i].decimation_filter.process(frameset).as_frameset() + frameset = cameras[i].depth_to_disparity.process(frameset).as_frameset() + frameset = cameras[i].spatial_filter.process(frameset).as_frameset() + frameset = cameras[i].temporal_filter.process(frameset).as_frameset() + frameset = cameras[i].disparity_to_depth.process(frameset).as_frameset() + # frameset = cameras[i].hole_filling_filter.process(frameset).as_frameset() + + if apply_align: + cameras[i].get_aligned_frames(frameset, aligned_to_color=True) + depth_frame = cameras[i].depth_frame_aligned + color_frame = cameras[i].color_frame_aligned + else: + depth_frame = frameset.get_depth_frame() + color_frame = frameset.get_color_frame() + + cameras[i].color_image = frame_to_np_array(color_frame) + cameras[i].colored_depthmap = frame_to_np_array(depth_frame, colorize_depth=True) + cameras[i].depth_image = frame_to_np_array(depth_frame) + + # Show image + for i in cameras: + image_name = '{}_filtered depth'.format(i.device.get_info(rs.camera_info.serial_number)) + cv2.imshow(image_name, i.colored_depthmap) + + key = cv2.waitKey(1) + + # Press esc or 'q' to close the image window + if key & 0xFF == ord('q') or key == 27: + cv2.destroyAllWindows() + break + + # if key & 0xFF == ord('d'): + # decimation_magnitude = cameras[0].decimation.options[OptionType.MAGNITUDE] + # cameras[0].decimation.increment(decimation_magnitude) +finally: + for i in cameras: + i.stop() diff --git a/common/RealSense_Utilities/realsense_api/__init__.py b/common/RealSense_Utilities/realsense_api/__init__.py new file mode 100644 index 0000000..56acdb5 --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/__init__.py @@ -0,0 +1 @@ +__all__ = ['realsense_api'] diff --git a/common/RealSense_Utilities/realsense_api/__pycache__/__init__.cpython-38.pyc b/common/RealSense_Utilities/realsense_api/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000..c997604 Binary files /dev/null and b/common/RealSense_Utilities/realsense_api/__pycache__/__init__.cpython-38.pyc differ diff --git a/common/RealSense_Utilities/realsense_api/__pycache__/realsense_api.cpython-38.pyc b/common/RealSense_Utilities/realsense_api/__pycache__/realsense_api.cpython-38.pyc new file mode 100644 index 0000000..e520e1d Binary files /dev/null and b/common/RealSense_Utilities/realsense_api/__pycache__/realsense_api.cpython-38.pyc differ diff --git a/common/RealSense_Utilities/realsense_api/d405.json b/common/RealSense_Utilities/realsense_api/d405.json new file mode 100644 index 0000000..4df7aa4 --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/d405.json @@ -0,0 +1,90 @@ +{ + "device": { + "fw version": "5.15.1", + "name": "Intel RealSense D405", + "product line": "D400" + }, + "parameters": { + "aux-param-autoexposure-setpoint": "1000", + "aux-param-colorcorrection1": "-0.0556641", + "aux-param-colorcorrection10": "-0.194336", + "aux-param-colorcorrection11": "-0.194336", + "aux-param-colorcorrection12": "-0.589844", + "aux-param-colorcorrection2": "0.560547", + "aux-param-colorcorrection3": "0.560547", + "aux-param-colorcorrection4": "0.170898", + "aux-param-colorcorrection5": "-0.275391", + "aux-param-colorcorrection6": "-0.238281", + "aux-param-colorcorrection7": "-0.238281", + "aux-param-colorcorrection8": "1.34766", + "aux-param-colorcorrection9": "0.959961", + "aux-param-depthclampmax": "65536", + "aux-param-depthclampmin": "0", + "aux-param-disparityshift": "0", + "controls-autoexposure-auto": "True", + "controls-autoexposure-manual": "33000", + "controls-depth-gain": "16", + "controls-depth-white-balance-auto": "True", + "ignoreSAD": "0", + "param-amplitude-factor": "0", + "param-autoexposure-setpoint": "1000", + "param-censusenablereg-udiameter": "9", + "param-censusenablereg-vdiameter": "9", + "param-censususize": "9", + "param-censusvsize": "9", + "param-depthclampmax": "65536", + "param-depthclampmin": "0", + "param-depthunits": "100", + "param-disableraucolor": "0", + "param-disablesadcolor": "0", + "param-disablesadnormalize": "0", + "param-disablesloleftcolor": "0", + "param-disableslorightcolor": "0", + "param-disparitymode": "0", + "param-disparityshift": "0", + "param-lambdaad": "800", + "param-lambdacensus": "26", + "param-leftrightthreshold": "24", + "param-maxscorethreshb": "2047", + "param-medianthreshold": "500", + "param-minscorethresha": "1", + "param-neighborthresh": "7", + "param-raumine": "1", + "param-rauminn": "1", + "param-rauminnssum": "3", + "param-raumins": "1", + "param-rauminw": "1", + "param-rauminwesum": "3", + "param-regioncolorthresholdb": "0.0499022", + "param-regioncolorthresholdg": "0.0499022", + "param-regioncolorthresholdr": "0.0499022", + "param-regionshrinku": "3", + "param-regionshrinkv": "1", + "param-robbinsmonrodecrement": "10", + "param-robbinsmonroincrement": "10", + "param-rsmdiffthreshold": "4", + "param-rsmrauslodiffthreshold": "1", + "param-rsmremovethreshold": "0.375", + "param-scanlineedgetaub": "72", + "param-scanlineedgetaug": "72", + "param-scanlineedgetaur": "72", + "param-scanlinep1": "60", + "param-scanlinep1onediscon": "105", + "param-scanlinep1twodiscon": "70", + "param-scanlinep2": "342", + "param-scanlinep2onediscon": "190", + "param-scanlinep2twodiscon": "130", + "param-secondpeakdelta": "325", + "param-texturecountthresh": "0", + "param-texturedifferencethresh": "0", + "param-usersm": "1", + "param-zunits": "100" + }, + "schema version": 1, + "viewer": { + "stream-depth-format": "Z16", + "stream-fps": "30", + "stream-height": "480", + "stream-width": "640" + } +} \ No newline at end of file diff --git a/common/RealSense_Utilities/realsense_api/d455.json b/common/RealSense_Utilities/realsense_api/d455.json new file mode 100644 index 0000000..1bd164f --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/d455.json @@ -0,0 +1,104 @@ +{ + "device": { + "fw version": "05.13.00.50", + "name": "Intel RealSense D455", + "product line": "D400" + }, + "parameters": { + "aux-param-autoexposure-setpoint": "1000", + "aux-param-colorcorrection1": "-0.493164", + "aux-param-colorcorrection10": "-0.272461", + "aux-param-colorcorrection11": "-0.272461", + "aux-param-colorcorrection12": "-0.355469", + "aux-param-colorcorrection2": "0.831055", + "aux-param-colorcorrection3": "0.831055", + "aux-param-colorcorrection4": "-0.368164", + "aux-param-colorcorrection5": "-0.133789", + "aux-param-colorcorrection6": "-0.323242", + "aux-param-colorcorrection7": "-0.323242", + "aux-param-colorcorrection8": "1.19141", + "aux-param-colorcorrection9": "0.90918", + "aux-param-depthclampmax": "1500", + "aux-param-depthclampmin": "0", + "aux-param-disparityshift": "0", + "controls-autoexposure-auto": "True", + "controls-autoexposure-manual": "3500", + "controls-color-autoexposure-auto": "True", + "controls-color-autoexposure-manual": "156", + "controls-color-backlight-compensation": "0", + "controls-color-brightness": "0", + "controls-color-contrast": "50", + "controls-color-gain": "64", + "controls-color-gamma": "300", + "controls-color-hue": "0", + "controls-color-power-line-frequency": "3", + "controls-color-saturation": "64", + "controls-color-sharpness": "50", + "controls-color-white-balance-auto": "True", + "controls-color-white-balance-manual": "4600", + "controls-depth-gain": "16", + "controls-laserpower": "360", + "controls-laserstate": "on", + "ignoreSAD": "0", + "param-amplitude-factor": "0.08", + "param-autoexposure-setpoint": "1000", + "param-censusenablereg-udiameter": "9", + "param-censusenablereg-vdiameter": "9", + "param-censususize": "9", + "param-censusvsize": "9", + "param-depthclampmax": "1500", + "param-depthclampmin": "0", + "param-depthunits": "1000", + "param-disableraucolor": "0", + "param-disablesadcolor": "0", + "param-disablesadnormalize": "0", + "param-disablesloleftcolor": "0", + "param-disableslorightcolor": "0", + "param-disparitymode": "0", + "param-disparityshift": "0", + "param-lambdaad": "800", + "param-lambdacensus": "26", + "param-leftrightthreshold": "24", + "param-maxscorethreshb": "2047", + "param-medianthreshold": "500", + "param-minscorethresha": "1", + "param-neighborthresh": "7", + "param-raumine": "1", + "param-rauminn": "1", + "param-rauminnssum": "3", + "param-raumins": "1", + "param-rauminw": "1", + "param-rauminwesum": "3", + "param-regioncolorthresholdb": "0.0499022", + "param-regioncolorthresholdg": "0.0499022", + "param-regioncolorthresholdr": "0.0499022", + "param-regionshrinku": "3", + "param-regionshrinkv": "1", + "param-robbinsmonrodecrement": "10", + "param-robbinsmonroincrement": "10", + "param-rsmdiffthreshold": "4", + "param-rsmrauslodiffthreshold": "1", + "param-rsmremovethreshold": "0.375", + "param-scanlineedgetaub": "72", + "param-scanlineedgetaug": "72", + "param-scanlineedgetaur": "72", + "param-scanlinep1": "60", + "param-scanlinep1onediscon": "105", + "param-scanlinep1twodiscon": "70", + "param-scanlinep2": "342", + "param-scanlinep2onediscon": "190", + "param-scanlinep2twodiscon": "130", + "param-secondpeakdelta": "325", + "param-texturecountthresh": "0", + "param-texturedifferencethresh": "0", + "param-usersm": "1", + "param-zunits": "1000" + }, + "schema version": 1, + "viewer": { + "stream-depth-format": "Z16", + "stream-fps": "90", + "stream-height": "480", + "stream-width": "848" + } +} \ No newline at end of file diff --git a/common/RealSense_Utilities/realsense_api/post_processing/__pycache__/option.cpython-38.pyc b/common/RealSense_Utilities/realsense_api/post_processing/__pycache__/option.cpython-38.pyc new file mode 100644 index 0000000..1474fad Binary files /dev/null and b/common/RealSense_Utilities/realsense_api/post_processing/__pycache__/option.cpython-38.pyc differ diff --git a/common/RealSense_Utilities/realsense_api/post_processing/__pycache__/options.cpython-38.pyc b/common/RealSense_Utilities/realsense_api/post_processing/__pycache__/options.cpython-38.pyc new file mode 100644 index 0000000..e89b507 Binary files /dev/null and b/common/RealSense_Utilities/realsense_api/post_processing/__pycache__/options.cpython-38.pyc differ diff --git a/common/RealSense_Utilities/realsense_api/post_processing/decimation.py b/common/RealSense_Utilities/realsense_api/post_processing/decimation.py new file mode 100644 index 0000000..0984414 --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/post_processing/decimation.py @@ -0,0 +1,19 @@ +# https://dev.intelrealsense.com/docs/post-processing-filters +from option import OptionValues, OptionType, FilterOptions + + +class DecimationOptions(FilterOptions): + def __init__(self) -> None: + self.options: OptionDict = { + OptionType.MAGNITUDE: OptionValues( + option_value=2, + option_value_increment=1, + option_min_value=1, + option_max_value=8 + ) + } + + def increment(self, option: OptionValues) -> None: + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value diff --git a/common/RealSense_Utilities/realsense_api/post_processing/hole_filling.py b/common/RealSense_Utilities/realsense_api/post_processing/hole_filling.py new file mode 100644 index 0000000..7de60fa --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/post_processing/hole_filling.py @@ -0,0 +1,19 @@ +# https://dev.intelrealsense.com/docs/post-processing-filters +from option import OptionValues, OptionType, FilterOptions + + +class HoleFillingOptions(FilterOptions): + def __init__(self) -> None: + self.options: OptionDict = { + OptionType.MAGNITUDE: OptionValues( + option_value=1, + option_value_increment=1, + option_min_value=0, + option_max_value=2 + ) + } + + def increment(self, option: OptionValues) -> None: + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value diff --git a/common/RealSense_Utilities/realsense_api/post_processing/option.py b/common/RealSense_Utilities/realsense_api/post_processing/option.py new file mode 100644 index 0000000..73aa770 --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/post_processing/option.py @@ -0,0 +1,31 @@ +from typing import TypedDict +from dataclasses import dataclass +from enum import Enum, auto +from abc import ABC, abstractmethod + + +class OptionType(Enum): + MAGNITUDE = auto() + SMOOTH_ALPHA = auto() + SMOOTH_DELTA = auto() + HOLE_FILLING = auto() + PERSISTENCY_INDEX = auto() + + +@dataclass +class OptionValues: + option_value: float + option_value_increment: float + option_min_value: float + option_max_value: float + + +# class OptionDict(TypedDict): +# option: OptionType +# properties: OptionValues + + +class FilterOptions(ABC): + @abstractmethod + def increment(self, option: OptionValues) -> None: + pass diff --git a/common/RealSense_Utilities/realsense_api/post_processing/options.py b/common/RealSense_Utilities/realsense_api/post_processing/options.py new file mode 100644 index 0000000..219759b --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/post_processing/options.py @@ -0,0 +1,112 @@ +# https://dev.intelrealsense.com/docs/post-processing-filters +from RealSense_Utilities.realsense_api.post_processing.option import OptionValues, OptionType, FilterOptions + + +############################################################# +# -------------------- DECIMATION OPTIONS -------------------# +############################################################# +class DecimationOptions(FilterOptions): + def __init__(self) -> None: + self.options: dict = { + OptionType.MAGNITUDE: OptionValues( + option_value=2, + option_value_increment=1, + option_min_value=1, + option_max_value=8 + ) + } + + def increment(self, option: OptionValues) -> None: + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value + + +############################################################# +# --------------------- SPATIAL OPTIONS ---------------------# +############################################################# +class SpatialOptions(FilterOptions): + def __init__(self) -> None: + self.options: dict = { + OptionType.MAGNITUDE: OptionValues( + option_value=2, + option_value_increment=1, + option_min_value=0, + option_max_value=5 + ), + OptionType.SMOOTH_ALPHA: OptionValues( + option_value=0.5, + option_value_increment=0.25, + option_min_value=0.0, + option_max_value=1.0 + ), + OptionType.SMOOTH_DELTA: OptionValues( + option_value=2, + option_value_increment=1, + option_min_value=0, + option_max_value=5 + ), + OptionType.HOLE_FILLING: OptionValues( + option_value=0, + option_value_increment=1, + option_min_value=0, + option_max_value=5 + ) + } + + def increment(self, option: OptionValues): + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value + + +############################################################# +# --------------------- TEMPORAL OPTIONS --------------------# +############################################################# +class TemporalOptions(FilterOptions): + def __init__(self) -> None: + self.options: dict = { + OptionType.SMOOTH_ALPHA: OptionValues( + option_value=0.4, + option_value_increment=0.1, + option_min_value=0.0, + option_max_value=1.0 + ), + OptionType.SMOOTH_DELTA: OptionValues( + option_value=20, + option_value_increment=10, + option_min_value=0, + option_max_value=100 + ), + OptionType.PERSISTENCY_INDEX: OptionValues( + option_value=3, + option_value_increment=1, + option_min_value=0, + option_max_value=8 + ) + } + + def increment(self, option: OptionValues) -> None: + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value + + +############################################################# +# ------------------- HOLE FILLING OPTIONS ------------------# +############################################################# +class HoleFillingOptions(FilterOptions): + def __init__(self) -> None: + self.options: dict = { + OptionType.HOLE_FILLING: OptionValues( + option_value=1, + option_value_increment=1, + option_min_value=0, + option_max_value=2 + ) + } + + def increment(self, option: OptionValues) -> None: + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value diff --git a/common/RealSense_Utilities/realsense_api/post_processing/spatial.py b/common/RealSense_Utilities/realsense_api/post_processing/spatial.py new file mode 100644 index 0000000..b6f73d3 --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/post_processing/spatial.py @@ -0,0 +1,64 @@ +# https://dev.intelrealsense.com/docs/post-processing-filters +from dataclasses import asdict, dataclass, field +from option import OptionValues, OptionType, OptionDict, FilterOptions + + +class SpatialOptions(FilterOptions): + def __init__(self) -> None: + self.options: OptionDict = { + OptionType.MAGNITUDE: OptionValues( + option_value=2, + option_value_increment=1, + option_min_value=0, + option_max_value=5 + ), + OptionType.SMOOTH_ALPHA: OptionValues( + option_value=0.5, + option_value_increment=0.25, + option_min_value=0.0, + option_max_value=1.0 + ), + OptionType.SMOOTH_DELTA: OptionValues( + option_value=2, + option_value_increment=1, + option_min_value=0, + option_max_value=5 + ), + OptionType.HOLE_FILLING: OptionValues( + option_value=0, + option_value_increment=1, + option_min_value=0, + option_max_value=5 + ) + } + + def increment(self, option: OptionValues): + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value + +# spatial_filter = SpatialOptions() +# spatial_filter2 = SpatialOptions() + +# # # spatial_options = asdict(spatial_filter.options) +# spatial_filter.options[OptionType.SMOOTH_ALPHA].option_value_increment = 0.1 +# spatial_filter2.options[OptionType.SMOOTH_ALPHA].option_value_increment = 0.25 + +# spatial_filter_hole_filling = spatial_filter.options[OptionType.SMOOTH_ALPHA] +# spatial_filter_hole_filling2 = spatial_filter2.options[OptionType.SMOOTH_ALPHA] + +# for i in range(20): +# print(f'{spatial_filter_hole_filling.option_value} {spatial_filter_hole_filling2.option_value}') +# spatial_filter.increment(spatial_filter_hole_filling) +# spatial_filter2.increment(spatial_filter_hole_filling2) + +# print(type(spatial_filter)) +# for i,v in spatial_options.items(): +# print(i,v) + +# =a.hole_filling +# print(o.option_value) +# for i in range(7): +# a.increment(o) +# for i in spatial_filter.options: +# print(i) diff --git a/common/RealSense_Utilities/realsense_api/post_processing/temporal.py b/common/RealSense_Utilities/realsense_api/post_processing/temporal.py new file mode 100644 index 0000000..c6ae54c --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/post_processing/temporal.py @@ -0,0 +1,31 @@ +# https://dev.intelrealsense.com/docs/post-processing-filters +from option import OptionValues, OptionType, OptionDict, FilterOptions + + +class TemporalOptions(FilterOptions): + def __init__(self) -> None: + self.options: OptionDict = { + OptionType.SMOOTH_ALPHA: OptionValues( + option_value=0.4, + option_value_increment=0.1, + option_min_value=0.0, + option_max_value=1.0 + ), + OptionType.SMOOTH_DELTA: OptionValues( + option_value=20, + option_value_increment=10, + option_min_value=0, + option_max_value=100 + ), + OptionType.HOLE_FILLING: OptionValues( + option_value=3, + option_value_increment=1, + option_min_value=0, + option_max_value=8 + ) + } + + def increment(self, option: OptionValues) -> None: + option.option_value += option.option_value_increment + if option.option_value > option.option_max_value: + option.option_value = option.option_min_value diff --git a/common/RealSense_Utilities/realsense_api/realsense_api.py b/common/RealSense_Utilities/realsense_api/realsense_api.py new file mode 100644 index 0000000..3816c2b --- /dev/null +++ b/common/RealSense_Utilities/realsense_api/realsense_api.py @@ -0,0 +1,374 @@ +import time + +import numpy as np +import pyrealsense2 as rs +import json +import cv2 +from threading import Thread +from multiprocessing.pool import ThreadPool +# from realsense_api.post_processing.option import OptionType +from RealSense_Utilities.realsense_api.post_processing.options import \ + DecimationOptions, SpatialOptions, TemporalOptions, HoleFillingOptions, OptionType + +import os, sys + +# TODO: +# - Create a method for loading the json options that are generated by the realsense viewer +# - Create a method for displaying the options that are currently set +# - Add any remaining stream types into the get data method such as confidence +# - Add any other post processing filters that are missing + +# Links that I thought would be useful later: +# https://intelrealsense.github.io/librealsense/python_docs/_generated/pyrealsense2.threshold_filter.html?highlight=process +# https://github.com/IntelRealSense/librealsense/issues/6902 +# https://github.com/IntelRealSense/librealsense/issues/10078 + +def frame_to_np_array(frame, colorize_depth=False): + # Create colorized depth frame + if colorize_depth: + colorizer = rs.colorizer() + frame_as_image = np.asanyarray(colorizer.colorize(frame).get_data()) + return frame_as_image + frame_as_image = np.asanyarray(frame.get_data()) + return frame_as_image + + +def find_realsense(): + realsense_ctx = rs.context() + connected_devices = [] + serial_number = [] + for i in range(len(realsense_ctx.devices)): + detected_camera = realsense_ctx.devices[i] + connected_devices.append(detected_camera) + serial_number.append(detected_camera.get_info(rs.camera_info.serial_number)) + + return zip(serial_number, connected_devices) + + +def mediapipe_detection(image, model): + image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) # COLOR CONVERSION BGR 2 RGB + image.flags.writeable = False # Image is no longer writeable + results = model.process(image) # Make prediction + image.flags.writeable = True # Image is now writeable + image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) # COLOR CONVERSION RGB 2 BGR + return image, results + + +class RealSenseCamera: + def __init__(self, + depth_stream_width=848, depth_stream_height=480, + color_stream_width=640, color_stream_height=480, + depth_stream_fps=90, color_stream_fps=60, + device=None, adv_mode_flag=False, device_type='d455', + mp_lock=None, disable_color_auto_exposure=False): + + self.device = device + self.device_type = device_type + + self.current_timestamp = 0 + + self.colored_depthmap = None + self.color_image = None + self.depth_image = None + + # Resolution attributes + self.depth_stream_width = depth_stream_width + self.depth_stream_height = depth_stream_height + self.color_stream_width = color_stream_width + self.color_stream_height = color_stream_height + + self.depth_stream_fps = depth_stream_fps + self.color_stream_fps = color_stream_fps + + # Data attributes that will be set with get_data() + self.frameset = None + self.depth_frame = None + self.color_frame = None + self.infrared_frame = None + self.color_intrinsics = None + self.depth_intrinsics = None + self.depth_scale = None + self.camera_matrix = None + self.dist_coeffs = None + + # Holds the frameset after it has undergone filtering + self.filtered_frameset = None + + # Aligned frame holders + self.depth_frame_aligned = None + self.color_frame_aligned = None + + # Post Processing Filter attributes with default values + # https://dev.intelrealsense.com/docs/post-processing-filters + + self.decimation: DecimationOptions = DecimationOptions() + self.spatial: SpatialOptions = SpatialOptions() + self.temporal: TemporalOptions = TemporalOptions() + self.hole_filling: HoleFillingOptions = HoleFillingOptions() + + # Decimation filter attribute + self.decimation_magnitude = 3 + + # Spatial filter attributes + self.spatial_magnitude = 2 + self.spatial_smooth_alpha = 0.4 + self.spatial_smooth_delta = 4 + self.spatial_holes_fill = 0 + + # Temporal filter attributes + self.temporal_smooth_alpha = 0.1 + self.temporal_smooth_delta = 20 + self.persistency_index = 5 + + # Holes Filling filter attribute + self.hole_filling_param = 1 # farest + # self.hole_filling_param = 2 # nearest + + self.decimation_filter = rs.decimation_filter() + self.decimation_filter.set_option( + rs.option.filter_magnitude, + self.decimation_magnitude + ) + + self.spatial_filter = rs.spatial_filter() + self.spatial_filter.set_option( + rs.option.filter_magnitude, + self.spatial_magnitude + ) + self.spatial_filter.set_option( + rs.option.filter_smooth_alpha, + self.spatial_smooth_alpha + ) + self.spatial_filter.set_option( + rs.option.filter_smooth_delta, + self.spatial_smooth_delta + ) + self.spatial_filter.set_option( + rs.option.holes_fill, + self.spatial_holes_fill + ) + + self.temporal_filter = rs.temporal_filter() + self.temporal_filter.set_option( + rs.option.filter_smooth_alpha, + self.temporal_smooth_alpha + ) + self.temporal_filter.set_option( + rs.option.filter_smooth_delta, + self.temporal_smooth_delta + ) + self.temporal_filter.set_option( + rs.option.holes_fill, + self.persistency_index + ) + + self.hole_filling_filter = rs.hole_filling_filter() + self.hole_filling_filter.set_option( + rs.option.holes_fill, + self.hole_filling_param + ) + + self.depth_to_disparity = rs.disparity_transform(True) + self.disparity_to_depth = rs.disparity_transform(False) + + # json profile + script_dir = os.path.dirname(os.path.abspath(__file__)) + if self.device_type == 'd455': + config_file_path = os.path.join(script_dir, 'd455.json') + self.jsonObj = json.load(open(config_file_path)) + # self.jsonObj = json.load(open("./d455.json")) + elif self.device_type == 'd405': + config_file_path = os.path.join(script_dir, 'd405.json') + self.jsonObj = json.load(open(config_file_path)) + # self.jsonObj = json.load(open("./d405_0321.json")) + elif self.device_type == 'd415': + self.jsonObj = json.load(open("./d415_0131.json")) + + + print(f"{self.device_type} is selected...") + + self.pipeline = rs.pipeline() + + # restart for initializing + # self.stop() + + config = rs.config() + config.enable_device(self.device.get_info(rs.camera_info.serial_number)) + + config.enable_stream(rs.stream.depth, + self.depth_stream_width, self.depth_stream_height, + rs.format.z16, self.depth_stream_fps) + + config.enable_stream(rs.stream.color, + self.color_stream_width, self.color_stream_height, + rs.format.bgr8, self.color_stream_fps) + + config.enable_stream(rs.stream.infrared, + rs.format.y8, + self.depth_stream_fps) + + self.profile = self.pipeline.start(config) + + self.sensor = self.device.query_sensors() + + self.mp_lock = mp_lock + + # Get depth scale + depth_sensor = self.profile.get_device().first_depth_sensor() + depth_sensor.set_option(rs.option.emitter_enabled, 1) + # depth_sensor.set_option(rs.option.visual_preset, 4) # 4 corresponds to 'Hand' preset + self.depth_scale = depth_sensor.get_depth_scale() + + self.color_intrinsics = self.profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() + self.depth_intrinsics = self.profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() + self.camera_matrix = np.array( + [[self.color_intrinsics.fx, 0, self.color_intrinsics.ppx], + [0, self.color_intrinsics.fy, self.color_intrinsics.ppy], + [0, 0, 1]], + dtype=np.float64) + self.dist_coeffs = np.array([self.color_intrinsics.coeffs], dtype=np.float64) + + if adv_mode_flag: + self.adv_mode() + + if disable_color_auto_exposure: + color_sensor = self.sensor[1] + color_sensor.set_option(rs.option.auto_exposure_priority, 0) + color_sensor.set_option(rs.option.enable_auto_exposure, 0) + + def __iter__(self): + return self.device.get_info(rs.camera_info.serial_number) + + def __del__(self): + # self.stop() + pass + + # def get_options(self): + def get_data(self): + if self.mp_lock is not None: + self.mp_lock.acquire() + + frameset = self.pipeline.wait_for_frames() + try: + self.current_timestamp = frameset.get_timestamp() + # self.frameset.keep() + self.depth_frame = frameset.get_depth_frame() + self.color_frame = frameset.get_color_frame() + self.infrared_frame = frameset.first(rs.stream.infrared) + + self.frameset = frameset + + except RuntimeError as e: + print(str(self.device.get_info(rs.camera_info.serial_number)) + " camera can't polling frame") + + if self.mp_lock is not None: + self.mp_lock.release() + + def get_aligned_frames(self, frameset, aligned_to_color=False, aligned_to_depth=False): + if aligned_to_color: + align_to = rs.stream.color + align = rs.align(align_to) + frameset = align.process(frameset) + + if aligned_to_depth: + align_to = rs.stream.depth + align = rs.align(align_to) + frameset = align.process(frameset) + + self.depth_frame_aligned = frameset.get_depth_frame() + self.color_frame_aligned = frameset.get_color_frame() + self.frameset = frameset + + def filter_depth_data(self, + enable_decimation=False, + enable_spatial=False, + enable_temporal=False, + enable_hole_filling=False): + + """Apply a cascade of filters on the depth frame""" + depth_to_disparity = rs.disparity_transform(True) + disparity_to_depth = rs.disparity_transform(False) + + frameset = self.frameset + # DECIMATION FILTER + if enable_decimation: + decimation = rs.decimation_filter() + decimation.set_option( + rs.option.filter_magnitude, + self.decimation.options[OptionType.MAGNITUDE].option_value + ) + frameset = decimation.process(frameset).as_frameset() + + # depth to disparity + frameset = depth_to_disparity.process(frameset).as_frameset() + + # SPATIAL FILTER + if enable_spatial: + spatial = rs.spatial_filter() + spatial.set_option( + rs.option.filter_magnitude, + self.spatial.options[OptionType.MAGNITUDE].option_value + ) + spatial.set_option( + rs.option.filter_smooth_alpha, + self.spatial.options[OptionType.SMOOTH_ALPHA].option_value + ) + spatial.set_option( + rs.option.filter_smooth_delta, + self.spatial.options[OptionType.SMOOTH_DELTA].option_value + ) + spatial.set_option( + rs.option.holes_fill, + self.spatial.options[OptionType.HOLE_FILLING].option_value + ) + frameset = spatial.process(frameset).as_frameset() + + # TEMPORAL FILTER + if enable_temporal: + temporal = rs.temporal_filter() + temporal.set_option( + rs.option.filter_smooth_alpha, + self.temporal.options[OptionType.SMOOTH_ALPHA].option_value + ) + temporal.set_option( + rs.option.filter_smooth_delta, + self.temporal.options[OptionType.SMOOTH_DELTA].option_value + ) + temporal.set_option( + rs.option.holes_fill, + self.temporal.options[OptionType.PERSISTENCY_INDEX].option_value + ) + frameset = temporal.process(frameset).as_frameset() + + # disparity to depth + frameset = disparity_to_depth.process(frameset).as_frameset() + + # HOLE FILLING + if enable_hole_filling: + hole_filling = rs.hole_filling_filter() + hole_filling.set_option( + rs.option.holes_fill, + self.hole_filling.options[OptionType.HOLE_FILLING].option_value + ) + frameset = hole_filling.process(frameset).as_frameset() + + self.filtered_frameset = frameset + + def adv_mode(self): + json_string = str(self.jsonObj).replace("'", '\"') + + advnc_mode_arg = rs.rs400_advanced_mode(self.device) + if not advnc_mode_arg.is_enabled(): + advnc_mode_arg.toggle_advanced_mode(True) + + for i in range(5): + try: + advnc_mode_arg.load_json(json_string) + break + except Exception as e: + print(f'[realsense_api.py] {e}. Retry ({i}/5)') + time.sleep(1) + + def stop(self): + print("[realsense api] pipeline.stop") + self.pipeline.stop() diff --git a/common/RealSense_Utilities/test.py b/common/RealSense_Utilities/test.py new file mode 100644 index 0000000..d6f77c3 --- /dev/null +++ b/common/RealSense_Utilities/test.py @@ -0,0 +1,4 @@ +from realsense_api.post_processing.decimation import DecimationOptions +from realsense_api.post_processing.spatial import SpatialOptions +from realsense_api.post_processing.temporal import TemporalOptions +from realsense_api.post_processing.hole_filling import HoleFillingOptions \ No newline at end of file diff --git a/common/__init__.py b/common/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/common/mediapipe_api/handlandmarks/__init__.py b/common/mediapipe_api/handlandmarks/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/common/mediapipe_api/handlandmarks/__pycache__/__init__.cpython-38.pyc b/common/mediapipe_api/handlandmarks/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000..90c24e6 Binary files /dev/null and b/common/mediapipe_api/handlandmarks/__pycache__/__init__.cpython-38.pyc differ diff --git a/common/mediapipe_api/handlandmarks/__pycache__/handlandmarks.cpython-38.pyc b/common/mediapipe_api/handlandmarks/__pycache__/handlandmarks.cpython-38.pyc new file mode 100644 index 0000000..04154a0 Binary files /dev/null and b/common/mediapipe_api/handlandmarks/__pycache__/handlandmarks.cpython-38.pyc differ diff --git a/common/mediapipe_api/handlandmarks/handlandmark_keypoints.json b/common/mediapipe_api/handlandmarks/handlandmark_keypoints.json new file mode 100644 index 0000000..05cb2a6 --- /dev/null +++ b/common/mediapipe_api/handlandmarks/handlandmark_keypoints.json @@ -0,0 +1,25 @@ +{ + "keypoints": { + "wrist": 0, + "thumb_cmc": 1, + "thumb_mcp": 2, + "thumb_ip": 3, + "thumb_tip": 4, + "index_finger_mcp": 5, + "index_finger_pip": 6, + "index_finger_dip": 7, + "index_finger_tip": 8, + "middle_finger_mcp": 9, + "middle_finger_pip": 10, + "middle_finger_dip": 11, + "middle_finger_tip": 12, + "ring_finger_mcp": 13, + "ring_finger_pip": 14, + "ring_finger_dip": 15, + "ring_finger_tip": 16, + "pinky_mcp": 17, + "pinky_pip": 18, + "pinky_dip": 19, + "pinky_tip": 20 + } +} \ No newline at end of file diff --git a/common/mediapipe_api/handlandmarks/handlandmarks.py b/common/mediapipe_api/handlandmarks/handlandmarks.py new file mode 100644 index 0000000..e1e23a3 --- /dev/null +++ b/common/mediapipe_api/handlandmarks/handlandmarks.py @@ -0,0 +1,362 @@ +import mediapipe as mp +import cv2 +import numpy as np +import pyrealsense2 as rs +import time +import os +import sys +import json +import queue +queue = queue.Queue() + +class HandLandmarks(): + def __init__(self): + script_dir = os.path.dirname(__file__) # 현재 스크립트의 디렉토리 경로 + file_path = os.path.join(script_dir, 'handlandmark_keypoints.json') + self.keypoints = json.load(open(file_path)) + # self.keypoints = json.load(open("handlandmark_keypoints.json")) + self.hand_pose = {} # tuple + self.hand_pose['index'] = None + self.hand_pose['score'] = None + self.hand_pose['label'] = None + self.hand_pose['landmarks'] = np.zeros((21, 3)) + self.hand_pose['world_landmarks'] = np.zeros((21, 3)) # calculate from camera instrinsic(Realsense) + self.hand_results = [] + pass + +class MediaPipeHandLandmarkDetector(HandLandmarks): + # pass + def __init__(self, image_width=640, image_height=480, replace_threshold=50): + super().__init__() + self.camera_to_hand_vector = {'x': 0, 'y': 0, 'z': 0} + self.hand_landmarks = np.zeros((21, 3)) + self.mp_drawing = mp.solutions.drawing_utils + self.mp_hands = mp.solutions.hands + self.hands = self.mp_hands.Hands(min_detection_confidence=0.8, + min_tracking_confidence=0.7) + self.canonical_points = np.zeros((21, 3)) + self.world_points = np.zeros((21, 3)) + self.world_points_iqr = np.zeros((21, 3)) + self.replace_points = np.zeros((21, 3)) + self.replace_threshold = replace_threshold + + self.palm_vector = {'x', 'y', 'z', 'r', 'p', 'y'} + + self.depth_intrinsics = None + + self.image_width = image_width + self.image_height = image_height + + self.color_image = np.zeros((image_height, image_width, 3)) + self.depth_image = np.zeros((image_height, image_width)) + self.drawing_image = np.zeros((image_height, image_width, 3)) # show mediapipe algorithm image + self.drawing_depth_image = np.zeros((image_height, image_width, 3)) # show mediapipe algorithm image + + self.hand_thickness = 10 # mm + self.pps = 0 + + def __del__(self): + pass + + def hand_detection(self, color_frame, depth_frame, depth_intrinsic=None): + ###################################################### + # Flip the image horizontally for a later selfie-view display, and convert + # the BGR image to RGB. + self.depth_intrinsics = depth_intrinsic + self.color_image = color_frame + # self.color_image = cv2.flip(self.color_image, 1) + self.color_image = cv2.cvtColor(self.color_image, cv2.COLOR_BGR2RGB) + self.depth_image = depth_frame + self.depth_image_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.depth_image, alpha=0.4), cv2.COLORMAP_JET) + + # To improve performance, optionally mark the image as not writeable to + # pass by reference. + self.color_image.flags.writeable = False + results = self.hands.process(self.color_image) + + # Draw the hand annotations on the image. + self.color_image.flags.writeable = True + self.color_image = cv2.cvtColor(self.color_image, cv2.COLOR_RGB2BGR) + + self.drawing_image = self.color_image + self.drawing_depth_image = self.depth_image_colormap + + self.hand_results.clear() + self.hand_pose = {key: None for key in self.hand_pose} + + s_time = time.time() + if results.multi_handedness: + for handedness, hand_landmarks in zip(results.multi_handedness, results.multi_hand_landmarks): + # dictionary clear + + classification = handedness.classification[0] + self.hand_pose['index'] = classification.index + self.hand_pose['score'] = classification.score + self.hand_pose['label'] = classification.label + + self.hand_landmarks = np.zeros((21, 3)) + for idx, landmrk in enumerate(hand_landmarks.landmark): + cx, cy, cz = landmrk.x, landmrk.y, landmrk.z + self.hand_landmarks[idx, 0] = cx + self.hand_landmarks[idx, 1] = cy + self.hand_landmarks[idx, 2] = cz + + canonical_points, world_points, world_points_iqr, replace_points, depth_avg = ( + self.compare_coordinate_canonical_with_world(self.hand_landmarks)) + + self.canonical_points = canonical_points + self.world_points = world_points + self.world_points_iqr = world_points_iqr + self.replace_points = replace_points + + self.hand_pose['landmarks'] = self.hand_landmarks + if self.depth_intrinsics != None: + self.hand_pose['world_landmarks'] = self.get_hand_world_xyz() + c, n_v = self.get_palm_pose(coord='replace') + c2, n_v2 = self.get_palm_pose(coord='canonical') + + # finally + self.hand_results.append(self.hand_pose) + + # drawing points on image + drawing_image = self.color_image + self.mp_drawing.draw_landmarks( + drawing_image, + hand_landmarks, + self.mp_hands.HAND_CONNECTIONS, + # mp_drawing_styles.get_default_hand_landmarks_style(), + # mp_drawing_styles.get_default_hand_connections_style() + ) + + img_depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.depth_image, alpha=0.3), + cv2.COLORMAP_JET) + drawing_depth_image = self.depth_image_colormap + self.mp_drawing.draw_landmarks( + drawing_depth_image, + hand_landmarks, + self.mp_hands.HAND_CONNECTIONS, + # mp_drawing_styles.get_default_hand_landmarks_style(), + # mp_drawing_styles.get_default_hand_connections_style() + ) + drawing_image = cv2.putText(img=drawing_image, + text=f'Depth(avg) = {depth_avg}', + org=(50, 50), + fontFace=cv2.FONT_HERSHEY_PLAIN, + fontScale=1, + color=(0,255,0), + thickness=2, + lineType=cv2.LINE_AA) + drawing_image = cv2.putText(img=drawing_image, + text=f'index finger depth = {world_points[7,2]}', + org=(50, 80), + fontFace=cv2.FONT_HERSHEY_PLAIN, + fontScale=1, + color=(0, 255, 0), + thickness=2, + lineType=cv2.LINE_AA) + self.drawing_image = drawing_image + self.drawing_depth_image = drawing_depth_image + + e_time = time.time() + if e_time != s_time: + self.pps = 1 / (e_time - s_time) + + + def compare_coordinate_canonical_with_world(self, hand_landmarks): + """ + Comparing world points(from camera depth frame) to canonical points(from mediapipe values) + :param hand_landmarks: (21,3) numpy array + :param replace_th: threshold for replacing point to canonical values + :return: list of values of all coordinates + """ + canonical_point = np.zeros((21, 3)) + hand_landmarks = np.asarray(hand_landmarks) + w, h = self.image_width, self.image_height + + canonical_point[:, 0] = [min(int(x * w), w - 1) for x in hand_landmarks[:, 0]] + canonical_point[:, 1] = [min(int(x * h), h - 1) for x in hand_landmarks[:, 1]] + canonical_point[:, 2] = [int(x * w) for x in hand_landmarks[:, 2]] + canonical_point = np.asarray(canonical_point, dtype=int) + + world_point = np.zeros((21, 3)) + world_point_iqr = np.zeros((21, 3)) + + # depth = np.zeros((1, 21)) + depth = self.depth_image[canonical_point[:, 1], canonical_point[:, 0]] + world_point[:, 0] = canonical_point[:, 0] + world_point[:, 1] = canonical_point[:, 1] + world_point[:, 2] = depth + world_point_iqr[:, 0] = canonical_point[:, 0] + world_point_iqr[:, 1] = canonical_point[:, 1] + world_point_iqr[:, 2] = self.replace_outliers_iqr_as_mean(depth, q1_rate=25, q3_rate=75, alpha=0) + + # world_point = np.asarray(world_point, dtype=int) + world_point = np.asarray(world_point) + # world_point = np.asarray(world_point_iqr) + + depth_avg = np.mean(world_point[:, 2]) + canonical_point[:, 2] = canonical_point[:, 2] + depth_avg + self.hand_thickness + + replace_points = world_point + xyz_distances = np.linalg.norm(world_point - canonical_point, axis=1) + replace_points[xyz_distances >= self.replace_threshold] = canonical_point[xyz_distances >= self.replace_threshold] + return canonical_point, world_point, world_point_iqr, replace_points, depth_avg + pass + + @staticmethod + def vector_to_rpy(x, y, z): + # Yaw (ψ) + yaw = np.arctan2(y, x) + # Pitch (θ) + pitch = np.arctan2(-z, np.sqrt(x ** 2 + y ** 2)) + # Roll (φ) is typically set to zero for a single vector + roll = 0.0 + return roll, pitch, yaw + + @staticmethod + def remove_outliers_iqr(data, q1_rate=25., q3_rate=75., alpha=1.5): + """ + # IQR 방법으로 이상치 제거 + :return: + """ + q1 = np.percentile(data, q1_rate) + q3 = np.percentile(data, q3_rate) + iqr = q3 - q1 + lower_bound = q1 - (alpha * iqr) + upper_bound = q3 + (alpha * iqr) + return data[(data >= lower_bound) & (data <= upper_bound)] + + @staticmethod + def replace_outliers_iqr_as_mean(data, q1_rate=25., q3_rate=75., alpha=0.5): + """ + # IQR 방법으로 이상치 제거하고 평균값으로 대체하는 함수 + :return: + """ + q1 = np.percentile(data, q1_rate) + q3 = np.percentile(data, q3_rate) + iqr = q3 - q1 + lower_bound = q1 - (alpha * iqr) + upper_bound = q3 + (alpha * iqr) + + # 이상치의 인덱스를 저장 + outliers_indices = np.where((data < lower_bound) | (data > upper_bound))[0] + + # 이상치가 아닌 값들로 평균을 계산 + mean_value = np.mean(data[(data >= lower_bound) & (data <= upper_bound)]) + + # 이상치를 평균값으로 대체 + data[outliers_indices] = mean_value + + # print(f'IQR: q1:{q1}, q3:{q3}, iqr:{iqr}, l_b:{lower_bound}, u_b:{upper_bound}, indices:{outliers_indices}, mean:{mean_value}') + return data + + def get_hand_keypoint_pixel_xyz(self, hand_index=0, keypoint='index_finger_tip', coord='replace'): + try: + keypoint_index = self.keypoints['keypoints'][keypoint] + + if coord == 'canonical': + landmarks = self.canonical_points + x = landmarks[keypoint_index, 0] + y = landmarks[keypoint_index, 1] + z = landmarks[keypoint_index, 2] + return np.array([x, y, z]) + + elif coord == 'replace': + landmarks = self.replace_points + x = landmarks[keypoint_index, 0] + y = landmarks[keypoint_index, 1] + z = landmarks[keypoint_index, 2] + return np.array([x, y, z]) + + elif coord == 'normalized': + landmarks = self.hand_results[hand_index]['landmarks'] + x = landmarks[keypoint_index, 0] + y = landmarks[keypoint_index, 1] + z = landmarks[keypoint_index, 2] + return np.array([x, y, z]) + + except Exception as e: + print(f'[mediapipe_hands.py] Exception error: {e}') + finally: + pass + + def get_hand_world_xyz(self): + """ + Obtain real distances x, y, z of each points from the camera in the world coordinate. + :unit: mm + :return: x, y, z values between each point and camera + """ + xyz_world = [] + for idx, xyz in enumerate(self.replace_points): + coordinate_xyz = rs.rs2_deproject_pixel_to_point(self.depth_intrinsics, [xyz[0], xyz[1]], xyz[2]) + xyz_world.append(coordinate_xyz) + + xyz_world = np.asarray(xyz_world, dtype=np.float64) + return xyz_world + + def get_palm_pose(self, coord='replace'): + """ + Find some points of hand landmarks + :param keypoint: handlandmark_keypoints.json + :param coord: canonical, replace, landmarks(normalized) + :return: x, y, z values + """ + p1 = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='wrist', coord=coord) + p2 = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='pinky_mcp', coord=coord) + p3 = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='index_finger_mcp', coord=coord) + + v1 = p2 - p1 + v2 = p3 - p1 + center_point = (p1 + p2 + p3) / 3. + normal_vector = np.cross(v1, v2) + normal_unit_vector = normal_vector / np.linalg.norm(normal_vector, 2) + + return center_point, normal_unit_vector + + @staticmethod + def get_top_n_values(self, arr, n=3): + arr = np.array(arr) + max_indices = np.argpartition(arr, -n)[-n:] # 가장 큰 값의 인덱스를 찾습니다. + max_values = np.partition(arr, -n)[-n:] # 배열에서 가장 큰 값 3개를 찾습니다. + return max_indices, max_values + + @staticmethod + def get_bottom_n_values(self, arr, n=3): + arr = np.array(arr) + min_indices = np.argpartition(arr, n)[:n] # 가장 작은 값의 인덱스를 찾습니다. + min_values = np.partition(arr, n)[:n] # 배열에서 가장 작은 값 3개를 찾습니다. + return min_indices, min_values + +def main(): + HLD = MediaPipeHandLandmarkDetector() + print(f"Created class: {HLD}") + test_image = cv2.imread("hsh.jpg") + + arbitrary_depth_image = np.random.randint(0, 601, (480, 640)) + HLD.hand_detection(test_image, arbitrary_depth_image,) + # cv2.imshow('test_image', test_image) + cv2.imshow('mp_image', HLD.drawing_image) + + test_image = cv2.imread("wja1.png") + + arbitrary_depth_image = np.random.randint(0, 601, (480, 640)) + HLD.hand_detection(test_image, arbitrary_depth_image, ) + # cv2.imshow('test_image', test_image) + cv2.imshow('mp2_image', HLD.drawing_image) + + test_image = cv2.imread("yj.jpg") + + arbitrary_depth_image = np.random.randint(0, 601, (480, 640)) + HLD.hand_detection(test_image, arbitrary_depth_image, ) + # cv2.imshow('test_image', test_image) + cv2.imshow('mp3_image', HLD.drawing_image) + + + cv2.waitKey(0) + cv2.destroyAllWindows() + + sys.exit() + +if __name__ == '__main__': + main() + diff --git a/common/mediapipe_api/handlandmarks/hsh.jpg b/common/mediapipe_api/handlandmarks/hsh.jpg new file mode 100644 index 0000000..519006c Binary files /dev/null and b/common/mediapipe_api/handlandmarks/hsh.jpg differ diff --git a/common/mediapipe_api/handlandmarks/mediapipe_hands_realsense_demo_legacy.py b/common/mediapipe_api/handlandmarks/mediapipe_hands_realsense_demo_legacy.py new file mode 100644 index 0000000..f7cbd0a --- /dev/null +++ b/common/mediapipe_api/handlandmarks/mediapipe_hands_realsense_demo_legacy.py @@ -0,0 +1,880 @@ +import mediapipe as mp +import cv2 +import numpy as np +import uuid +import os +import pyrealsense2 as rs +import time +import matplotlib +import matplotlib.pyplot as plt +matplotlib.use('TkAgg') +from mpl_toolkits.mplot3d import Axes3D +from matplotlib.animation import FuncAnimation + +from functools import partial +import threading +from multiprocessing import Process +# matplotlib.use('Agg') +import json +import queue + +queue = queue.Queue() +g_hand_data = np.zeros((21, 3)) +g_canonical_points = np.zeros((21, 3)) +g_world_points = np.zeros((21, 3)) +g_world_points_iqr = np.zeros((21, 3)) +g_replace_point = np.zeros((21, 3)) +g_palm_center = np.zeros(3) +g_palm_normal_vector = np.zeros(3) +g_palm_center_cano = np.zeros(3) +g_palm_normal_vector_cano = np.zeros(3) + +class RealTimePlot3D: + def __init__(self, num_points=21): + self.num_points = num_points + self.data = np.random.randn(self.num_points, 3) + self.fig = plt.figure() + # self.fig2 = plt.figure() + self.ax = self.fig.add_subplot(231, projection='3d') # visualization of hand pose + self.ax.set_xlabel('X Label') + self.ax.set_ylabel('Y Label') + self.ax.set_zlabel('Z Label') + self.ax.set_xlim(-0.1, 0.1) + self.ax.set_ylim(-0.1, 0.1) + self.ax.set_zlim(-0.1, 0.1) + + self.ax2 = self.fig.add_subplot(232, projection='3d') # visualization of hand pose + self.ax2.set_xlabel('X pixel') + self.ax2.set_ylabel('Y pixel') + self.ax2.set_zlabel('Z depth') + + self.ax3 = self.fig.add_subplot(233, projection='3d') # visualization of hand pose + self.ax3.set_xlabel('X pixel') + self.ax3.set_ylabel('Y pixel') + self.ax3.set_zlabel('Z depth') + + self.ax4 = self.fig.add_subplot(234, projection='3d') # visualization of hand pose + self.ax4.set_xlabel('X pixel') + self.ax4.set_ylabel('Y pixel') + self.ax4.set_zlabel('Z depth') + + self.ax5 = self.fig.add_subplot(235, projection='3d') # visualization of hand pose + self.ax5.set_xlabel('X pixel') + self.ax5.set_ylabel('Y pixel') + self.ax5.set_zlabel('Z depth') + + self.ax6 = self.fig.add_subplot(236, projection='3d') # visualization of hand pose + self.ax6.set_xlabel('X pixel') + self.ax6.set_ylabel('Y pixel') + self.ax6.set_zlabel('Z depth') + + # self.ani = None + self.ani = FuncAnimation(self.fig, self.update_data, frames=1, interval=100) + self.scatter = self.ax.scatter(g_hand_data[:, 0], g_hand_data[:, 1], g_hand_data[:, 2]) + self.scatter_canonical_points = self.ax.scatter(g_canonical_points[:, 0], g_canonical_points[:, 1], g_canonical_points[:, 2]) + self.scatter_world_points = self.ax.scatter(g_world_points[:, 0], g_world_points[:, 1], g_world_points[:, 2]) + self.scatter_world_points_iqr = self.ax.scatter(g_world_points_iqr[:, 0], g_world_points_iqr[:, 1], g_world_points_iqr[:, 2]) + self.scatter_replace_points = self.ax.scatter(g_replace_point[:, 0], g_replace_point[:, 1], g_replace_point[:, 2]) + self.update_thread = threading.Thread(target=self.update_data) + self.update_thread.daemon = True + + def update_data(self, frames): + + global g_hand_data, g_canonical_points, g_world_points, g_world_points_iqr, g_replace_point + global g_palm_center, g_palm_normal_vector, g_palm_center_cano, g_palm_normal_vector_cano + colors = ['black', 'blue', 'green', 'orange', 'red', 'black'] + intervals = [4, 8, 12, 16, 20] + + while True: + + data = g_hand_data + data_c_p = g_canonical_points + data_w_p = g_world_points + data_w_p_iqr = g_world_points_iqr + data_r_p = g_replace_point + # print(f'canonical: {data_c_p}') + # print(f'world_iqr: {data_w_p_iqr}') + # print(f'replace: {data_r_p}') + + # 현재 점 삭제 + self.scatter.remove() + self.scatter_canonical_points.remove() + self.scatter_world_points.remove() + self.scatter_world_points_iqr.remove() + self.scatter_replace_points.remove() + self.ax.cla() + self.ax2.cla() + self.ax3.cla() + self.ax4.cla() + self.ax5.cla() + self.ax6.cla() + self.ax.set_xlabel('X Label') + self.ax.set_ylabel('Y Label') + self.ax.set_zlabel('Z Label') + # self.ax.set_xlim(0, 500) + # self.ax.set_ylim(0, 500) + # self.ax.set_zlim(-100, 400) + + max_xyz_c = np.max(data_c_p, axis=0) + min_xyz_c = np.min(data_c_p, axis=0) + max_xyz_w = np.max(data_w_p, axis=0) + min_xyz_w = np.min(data_w_p, axis=0) + max_xyz_w_iqr = np.max(data_w_p_iqr, axis=0) + min_xyz_w_iqr = np.min(data_w_p_iqr, axis=0) + + th = 50 + # self.ax2.set_xlim(min_xyz_c[0] - th, max_xyz_c[0] + th) + # self.ax2.set_ylim(min_xyz_c[1] - th, max_xyz_c[1] + th) + # self.ax2.set_zlim(min_xyz_c[2] - th, max_xyz_c[2] + th) + # self.ax4.set_xlim(min_xyz_c[0] - th, max_xyz_c[0] + th) + # self.ax4.set_ylim(min_xyz_c[1] - th, max_xyz_c[1] + th) + # self.ax4.set_zlim(min_xyz_c[2] - th, max_xyz_c[2] + th) + # self.ax5.set_xlim(min_xyz_w[0] - th, max_xyz_w[0] + th) + # self.ax5.set_ylim(min_xyz_w[1] - th, max_xyz_w[1] + th) + # self.ax5.set_zlim(min_xyz_w[2] - th, max_xyz_w[2] + th) + # self.ax6.set_xlim(min_xyz_w_iqr[0] - th, max_xyz_w_iqr[0] + th) + # self.ax6.set_ylim(min_xyz_w_iqr[1] - th, max_xyz_w_iqr[1] + th) + # self.ax6.set_zlim(min_xyz_w_iqr[2] - th, max_xyz_w_iqr[2] + th) + + self.ax2.set_xlim(300, 500) + self.ax2.set_ylim(100, 300) + self.ax2.set_zlim(400, 700) + self.ax3.set_xlim(300, 500) + self.ax3.set_ylim(100, 300) + self.ax3.set_zlim(400, 700) + self.ax4.set_xlim(300, 500) + self.ax4.set_ylim(100, 300) + self.ax4.set_zlim(400, 700) + self.ax5.set_xlim(300, 500) + self.ax5.set_ylim(100, 300) + self.ax5.set_zlim(400, 700) + self.ax6.set_xlim(300, 500) + self.ax6.set_ylim(100, 300) + self.ax6.set_zlim(400, 700) + + + # print(f'{data[8,0]} / {data[8,1]} / {data[8,2]}') + self.scatter = self.ax.scatter(data[:, 0], data[:, 1], data[:, 2], color='black', s=50, alpha=1) + + self.scatter_canonical_points = self.ax2.scatter(data_c_p[:, 0], data_c_p[:, 1], data_c_p[:, 2], color='red', s=10, alpha=1) + self.scatter_world_points = self.ax2.scatter(data_w_p[:, 0], data_w_p[:, 1], data_w_p[:, 2], color='blue', s=10, alpha=1) + self.scatter_world_points_iqr = self.ax2.scatter(data_w_p_iqr[:, 0], data_w_p_iqr[:, 1], data_w_p_iqr[:, 2], color='green', s=10, alpha=1) + self.scatter_replace_points = self.ax2.scatter(data_r_p[:, 0], data_r_p[:, 1], data_r_p[:, 2], color='purple', s=10, alpha=1) + + self.scatter_replace_points = self.ax3.scatter(data_r_p[:, 0], data_r_p[:, 1], data_r_p[:, 2], color='purple', s=10, alpha=1) + + self.scatter_canonical_points = self.ax4.scatter(data_c_p[:, 0], data_c_p[:, 1], data_c_p[:, 2], color='red', s=10, alpha=1) + + self.scatter_world_points = self.ax5.scatter(data_w_p[:, 0], data_w_p[:, 1], data_w_p[:, 2], color='blue', s=10, alpha=1) + + self.scatter_world_points_iqr = self.ax6.scatter(data_w_p_iqr[:, 0], data_w_p_iqr[:, 1], data_w_p_iqr[:, 2], color='green', s=10, alpha=1) + # self.scatter_replace_points = self.ax6.scatter(data_r_p[:, 0], data_r_p[:, 1], data_r_p[:, 2], color='purple', s=10, alpha=1) + + # print(f'canonical: {data_c_p}') + # print(f'world_iqr: {data_w_p_iqr}') + # print(f'replace: {data_r_p}') + # self.scatter_2 = self.ax2.scatter() + for i in range(len(intervals)): + start_idx = 0 if i == 0 else intervals[i - 1] + 1 + end_idx = intervals[i] + self.ax.plot(data[start_idx:end_idx + 1, 0], data[start_idx:end_idx + 1, 1], data[start_idx:end_idx + 1, 2], color=colors[i]) + self.ax2.plot(data_c_p[start_idx:end_idx + 1, 0], data_c_p[start_idx:end_idx + 1, 1], data_c_p[start_idx:end_idx + 1, 2], color='red') + self.ax2.plot(data_w_p[start_idx:end_idx + 1, 0], data_w_p[start_idx:end_idx + 1, 1], data_w_p[start_idx:end_idx + 1, 2], color='blue') + self.ax2.plot(data_w_p_iqr[start_idx:end_idx + 1, 0], data_w_p_iqr[start_idx:end_idx + 1, 1], data_w_p_iqr[start_idx:end_idx + 1, 2], color='green') + self.ax2.plot(data_r_p[start_idx:end_idx + 1, 0], data_r_p[start_idx:end_idx + 1, 1], data_r_p[start_idx:end_idx + 1, 2], color='purple') + + self.ax3.plot(data_r_p[start_idx:end_idx + 1, 0], data_r_p[start_idx:end_idx + 1, 1], data_r_p[start_idx:end_idx + 1, 2], color='purple') + # 벡터 그리기 + self.ax3.quiver(g_palm_center[0], g_palm_center[1], g_palm_center[2] , g_palm_normal_vector[0], g_palm_normal_vector[1], g_palm_normal_vector[2], color='black', length=150, normalize=False) + + self.ax4.plot(data_c_p[start_idx:end_idx + 1, 0], data_c_p[start_idx:end_idx + 1, 1], data_c_p[start_idx:end_idx + 1, 2], color='red') + self.ax4.quiver(g_palm_center_cano[0], g_palm_center_cano[1], g_palm_center_cano[2] , g_palm_normal_vector_cano[0], g_palm_normal_vector_cano[1], g_palm_normal_vector_cano[2], color='black', length=150, normalize=False) + + self.ax5.plot(data_w_p[start_idx:end_idx + 1, 0], data_w_p[start_idx:end_idx + 1, 1], data_w_p[start_idx:end_idx + 1, 2], color='blue') + + self.ax6.plot(data_w_p_iqr[start_idx:end_idx + 1, 0], data_w_p_iqr[start_idx:end_idx + 1, 1], data_w_p_iqr[start_idx:end_idx + 1, 2], color='green') + # self.ax6.plot(data_r_p[start_idx:end_idx + 1, 0], data_r_p[start_idx:end_idx + 1, 1], data_r_p[start_idx:end_idx + 1, 2], color='purple') + + return + # return self.scatter + + def start(self): + # 데이터 업데이트 쓰레드 시작 + self.update_thread.start() + self.ani = FuncAnimation(self.fig, self.update_data, frames=100, interval=30) + + +class HandLandmarks(): + def __init__(self): + self.keypoints = json.load(open("handlandmark_keypoints.json")) + self.hand_landmarks = {} # tuple + self.hand_landmarks['index'] = None + self.hand_landmarks['score'] = None + self.hand_landmarks['label'] = None + self.hand_landmarks['landmarks'] = np.zeros((21, 3)) + self.hand_landmarks['world_landmarks'] = np.zeros((21, 3)) # calculate from camera instrinsic(Realsense) + # self.hand_landmarks['canonical_landmarks'] = None # calculate from camera instrinsic(Realsense) + # self.hand_landmarks['replace_landmarks'] = None # calculate from camera instrinsic(Realsense) + + + # if serveral hands is detected, hand_results will has objects of each hands. + # e.g. [{hand_landmarks#1}, {hand_landmarks#2}, ...] + self.hand_results = [] + + pass + + +class MediaPipeHandLandmarkDetector(HandLandmarks): + # pass + def __init__(self): + super().__init__() + self.camera_to_hand_vector = {'x': 0, 'y': 0, 'z': 0} + self.hand_data = np.zeros((21, 3)) + self.mp_drawing = mp.solutions.drawing_utils + self.mp_hands = mp.solutions.hands + self.hands = self.mp_hands.Hands(min_detection_confidence=0.8, + min_tracking_confidence=0.7) + self.canonical_points = np.zeros((21, 3)) + self.world_points = np.zeros((21, 3)) + self.world_points_iqr = np.zeros((21, 3)) + self.replace_point = np.zeros((21, 3)) + self.palm_vector = {'x', 'y', 'z', 'r','p','y'} + + # smoothing (LPF-low pass filter) + self.depth_intrinsics = None + self.finger_depth_prev = 0 + self.finger_depth_curr = 0 + self.filter_sensitivity = 0.3 + + self.image_width = 640 + self.image_height = 480 + + self.color_image = None + self.depth_image = None + self.depth_image_prev = None + self.depth_colormap = None + self.blended_image = None + self.start_flag = False + + self.hand_thickness = 10 # mm + + self.pps = 0 + + def hand_detection(self, color_frame, depth_frame, depth_intrinsic=None): + global g_hand_data, g_canonical_points, g_world_points, g_world_points_iqr, g_replace_point + global g_palm_center, g_palm_normal_vector, g_palm_center_cano, g_palm_normal_vector_cano + ###################################################### + # Flip the image horizontally for a later selfie-view display, and convert + # the BGR image to RGB. + self.depth_intrinsics = depth_intrinsic + self.color_image = color_frame + self.color_image = cv2.flip(self.color_image, 1) + self.color_image = cv2.cvtColor(self.color_image, cv2.COLOR_BGR2RGB) + self.depth_image = depth_frame + + if self.start_flag == False: + self.depth_image_filtered = self.depth_image + else: + self.depth_image_filtered = self.filter_sensitivity * self.depth_image + ( + 1 - self.filter_sensitivity) * self.depth_image_prev + + self.depth_image_filtered = cv2.flip(self.depth_image_filtered, 1) + self.depth_image_prev = self.depth_image_filtered + # depth_norm_image = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.3), cv2.COLORMAP_JET) + + # To improve performance, optionally mark the image as not writeable to + # pass by reference. + self.color_image.flags.writeable = False + results = self.hands.process(self.color_image) + self.image_height, self.image_width, _ = self.color_image.shape + depth_image_height, depth_image_width = self.depth_image_filtered.shape + if self.image_height != depth_image_height or self.image_width != depth_image_width: + print(f'[mediapipe_hands.py | Warning] It does not match a size (H x W) of color & depth frame') + # Draw the hand annotations on the image. + self.color_image.flags.writeable = True + self.color_image = cv2.cvtColor(self.color_image, cv2.COLOR_RGB2BGR) + ###################################################### + + self.hand_results.clear() + + if results.multi_handedness: + + s_time = time.time() + + for handedness, hand_landmarks in zip(results.multi_handedness, results.multi_hand_landmarks): + # dictionary clear + self.hand_landmarks = {key: None for key in self.hand_landmarks} + + classification = handedness.classification[0] + index = classification.index + score = classification.score + label = classification.label + self.hand_landmarks['index'] = classification.index + self.hand_landmarks['score'] = classification.score + self.hand_landmarks['label'] = classification.label + + hand_data = np.zeros((21, 3)) + for idx, landmrk in enumerate(hand_landmarks.landmark): + cx, cy, cz = landmrk.x, landmrk.y, landmrk.z + hand_data[idx, 0] = cx + hand_data[idx, 1] = cy + hand_data[idx, 2] = cz + g_hand_data[idx, 0] = cx + g_hand_data[idx, 1] = cy + g_hand_data[idx, 2] = cz + pixel_x = int(cx * self.image_width) + pixel_y = int(cy * self.image_height) + + canonical_points, world_points, world_points_iqr, replace_point = self.compare_coordinate_canonical_with_world(hand_data) + g_canonical_points = canonical_points + g_world_points = world_points + g_world_points_iqr = world_points_iqr + g_replace_point = replace_point + self.canonical_points = canonical_points + self.world_points = world_points + self.world_points_iqr = world_points_iqr + self.replace_point = replace_point + + + self.hand_landmarks['landmarks'] = hand_data + # world_landmarks = self.conversion_hand_keypoint_pixel_to_point(hand_data, top_point_n=None) + # self.hand_landmarks['world_landmarks'] = world_landmarks + self.hand_landmarks['world_landmarks'] = self.get_hand_world_xyz() + c, n_v = self.get_palm_pose(coord='replace') + g_palm_center = c + g_palm_normal_vector = n_v + + c2, n_v2 = self.get_palm_pose(coord='canonical') + g_palm_center_cano = c2 + g_palm_normal_vector_cano = n_v2 + + # finally + self.hand_results.append(self.hand_landmarks) + + # drawing points on image + self.mp_drawing.draw_landmarks( + self.color_image, + hand_landmarks, + self.mp_hands.HAND_CONNECTIONS, + # mp_drawing_styles.get_default_hand_landmarks_style(), + # mp_drawing_styles.get_default_hand_connections_style() + ) + + # self.depth_colormap = np.zeros((self.image_height, self.image_width, 3), dtype=np.uint16) + # self.depth_colormap[:, :, 0] = self.depth_image_filtered + # self.depth_colormap = cv2.cvtColor(self.depth_colormap, cv2.COLOR_RGB2BGR) + + self.depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.depth_image_filtered, alpha=0.3), cv2.COLORMAP_JET) + # self.depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(self.depth_colormap, alpha=0.3), cv2.COLORMAP_JET) + + # 이미지 블렌딩 + alpha = 0.3 # 컬러 이미지의 투명도 + beta = 1 - alpha # 깊이 이미지의 투명도 + gamma = 0 # 추가적인 밝기 조절 + self.blended_image = cv2.addWeighted(self.color_image, alpha, self.depth_colormap, beta, gamma) + + + # self.mp_drawing.draw_landmarks( + # self.depth_colormap, + # hand_landmarks, + # self.mp_hands.HAND_CONNECTIONS, + # # mp_drawing_styles.get_default_hand_landmarks_style(), + # # mp_drawing_styles.get_default_hand_connections_style() + # ) + + e_time = time.time() + self.pps = 1 / (e_time - s_time) + + def compare_coordinate_canonical_with_world(self, hand_landmarks_array, replace_th=50): + canonical_point = np.zeros((21, 3)) + hand_landmarks_array = np.asarray(hand_landmarks_array) + w, h = self.image_width, self.image_height + + # canonical_point[:, 0] = min(int(hand_landmarks_array[:, 0] * w), w - 1) + # canonical_point[:, 1] = min(int(hand_landmarks_array[:, 1] * h), h - 1) + # canonical_point[:, 2] = int(hand_landmarks_array[:, 2] * w) + canonical_point[:, 0] = [min(int(x * w), w - 1) for x in hand_landmarks_array[:, 0]] + canonical_point[:, 1] = [min(int(x * h), h - 1) for x in hand_landmarks_array[:, 1]] + canonical_point[:, 2] = [int(x * w) for x in hand_landmarks_array[:, 2]] + canonical_point = np.asarray(canonical_point, dtype=int) + + world_point = np.zeros((21, 3)) + world_point_iqr = np.zeros((21, 3)) + + # depth = np.zeros((1, 21)) + depth = self.depth_image_filtered[canonical_point[:, 1], canonical_point[:, 0]] + world_point[:, 0] = canonical_point[:, 0] + world_point[:, 1] = canonical_point[:, 1] + world_point[:, 2] = depth + world_point_iqr[:, 0] = canonical_point[:, 0] + world_point_iqr[:, 1] = canonical_point[:, 1] + world_point_iqr[:, 2] = self.replace_outliers_iqr_as_mean(depth, q1_rate=25, q3_rate=75, alpha=0) + + # world_point = np.asarray(world_point, dtype=int) + world_point = np.asarray(world_point) + + # depth_avg = np.mean(world_point[:, 2]) + depth_avg = np.mean(world_point_iqr[:, 2]) + canonical_point[:, 2] = canonical_point[:, 2] + depth_avg + self.hand_thickness + + replace_point = world_point_iqr + xyz_distances = np.linalg.norm(world_point_iqr-canonical_point, axis=1) + # print(f'canonical_point: {canonical_point}') + # print(f'world_point_iqr: {world_point_iqr}') + # print(f'replace_point(before): {replace_point}') + replace_point[xyz_distances >= replace_th] = canonical_point[xyz_distances >= replace_th] + # print(f'distances(th=20 mm): {xyz_distances} / {xyz_distances >= replace_th}') + # print(f'replace_point(after): {replace_point}') + return canonical_point, world_point, world_point_iqr, replace_point + pass + + @staticmethod + def vector_to_rpy(x, y, z): + # Yaw (ψ) + yaw = np.arctan2(y, x) + # Pitch (θ) + pitch = np.arctan2(-z, np.sqrt(x ** 2 + y ** 2)) + # Roll (φ) is typically set to zero for a single vector + roll = 0.0 + return roll, pitch, yaw + + @staticmethod + def remove_outliers_iqr(data, q1_rate=25., q3_rate=75., alpha=1.5): + """ + # IQR 방법으로 이상치 제거 + :return: + """ + q1 = np.percentile(data, q1_rate) + q3 = np.percentile(data, q3_rate) + iqr = q3 - q1 + lower_bound = q1 - (alpha * iqr) + upper_bound = q3 + (alpha * iqr) + return data[(data >= lower_bound) & (data <= upper_bound)] + + @staticmethod + def replace_outliers_iqr_as_mean(data, q1_rate=25., q3_rate=75., alpha=0.5): + """ + # IQR 방법으로 이상치 제거하고 평균값으로 대체하는 함수 + :return: + """ + q1 = np.percentile(data, q1_rate) + q3 = np.percentile(data, q3_rate) + iqr = q3 - q1 + lower_bound = q1 - (alpha * iqr) + upper_bound = q3 + (alpha * iqr) + + # 이상치의 인덱스를 저장 + outliers_indices = np.where((data < lower_bound) | (data > upper_bound))[0] + + # 이상치가 아닌 값들로 평균을 계산 + mean_value = np.mean(data[(data >= lower_bound) & (data <= upper_bound)]) + + # 이상치를 평균값으로 대체 + data[outliers_indices] = mean_value + + # print(f'IQR: q1:{q1}, q3:{q3}, iqr:{iqr}, l_b:{lower_bound}, u_b:{upper_bound}, indices:{outliers_indices}, mean:{mean_value}') + return data + + + def get_hand_keypoint_pixel_xyz(self, hand_index=0, keypoint='index_finger_tip', coord='replace'): + try: + keypoint_index = self.keypoints['keypoints'][keypoint] + + if coord == 'canonical': + landmarks = self.canonical_points + x = landmarks[keypoint_index, 0] + y = landmarks[keypoint_index, 1] + z = landmarks[keypoint_index, 2] + return np.array([x, y, z]) + + elif coord == 'replace': + landmarks = self.replace_point + x = landmarks[keypoint_index, 0] + y = landmarks[keypoint_index, 1] + z = landmarks[keypoint_index, 2] + return np.array([x, y, z]) + + elif coord == 'normalized': + landmarks = self.hand_results[hand_index]['landmarks'] + x = landmarks[keypoint_index, 0] + y = landmarks[keypoint_index, 1] + z = landmarks[keypoint_index, 2] + return np.array([x, y, z]) + + except Exception as e: + print(f'[mediapipe_hands.py] Exception error: {e}') + finally: + pass + + def get_hand_world_xyz(self): + xyz_world = [] + for idx, xyz in enumerate(self.replace_point): + coordinate_xyz = rs.rs2_deproject_pixel_to_point(self.depth_intrinsics, [xyz[0], xyz[1]], xyz[2]) + xyz_world.append(coordinate_xyz) + + xyz_world = np.asarray(xyz_world, dtype=np.float64) + return xyz_world + + def get_palm_pose(self, coord='replace'): + p1 = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='wrist', coord=coord) + p2 = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='pinky_mcp', coord=coord) + p3 = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='index_finger_mcp', coord=coord) + + v1 = p2 - p1 + v2 = p3 - p1 + center_point = (p1 + p2 + p3) / 3. + normal_vector = np.cross(v1, v2) + normal_unit_vector = normal_vector / np.linalg.norm(normal_vector, 2) + + return center_point, normal_unit_vector + + def conversion_hand_keypoint_pixel_to_point(self, keypoints_array, top_point_n=None): + """Algorithm for annotating pixel. + -- Not Used -- + Select 3(or more, optional) points closest to the camera. + And, calculate of scale of x,y and z (distance_pixel : distance_world) + Finally, obtain all 21 x,y and z dimension values of keypoints on real world + + Landmarks + There are 21 hand landmarks, each composed of x, y and z coordinates. + The x and y coordinates are normalized to [0.0, 1.0] by the image width and height, respectively. + The z coordinate represents the landmark depth, with the depth at the wrist being the origin. + The smaller the value, the closer the landmark is to the camera. + The magnitude of z uses roughly the same scale as x. + source: https://developers.google.com/mediapipe/solutions/vision/hand_landmarker/python#handle_and_display_results + + + Author: DY + Args: + keypoints_array (ndarray): (21, 3) array + + Returns: + ndarray: (21,3) world dx,dy,dz coordinates + """ + + try: + keypoint_arr = np.reshape(keypoints_array, (21, 3)) # must be check + + if top_point_n == None: + align_z_keypoint_world_arr = [] + h, w = self.depth_image_filtered.shape + for idx, pixel_xyz in enumerate(keypoint_arr): + x = min(int(pixel_xyz[0] * w), w - 1) # prevent the index overflow + y = min(int(pixel_xyz[1] * h), h - 1) # prevent the index overflow + if x <= 0 or x >= w or y <= 0 or y >= h: + ''' out of index ''' + return + + z = self.depth_image_filtered[y, x] + xyz_world = rs.rs2_deproject_pixel_to_point(self.depth_intrinsics, [x, y], z) + align_z_keypoint_world_arr.append(xyz_world) + + align_z_keypoint_world_arr = np.asarray(align_z_keypoint_world_arr, dtype=np.float64) + return align_z_keypoint_world_arr + + elif isinstance(top_point_n, int): + z_arr = keypoint_arr[:, 2] # depth values + min_z = np.min(z_arr) + min_z_index = np.argmin(z_arr) + ########################################################## + ## select top N values and obtain scale of x,y and z + # - start + b_ids, b_values = self.get_bottom_n_values(z_arr, n=top_point_n) + + selected_keypoint_arr = keypoint_arr[b_ids] + + selected_keypoint_world_arr = [] + offset_min_z = 0 + h, w = self.depth_image_filtered.shape + for idx, pixel_xyz in enumerate(selected_keypoint_arr): + x = min(int(pixel_xyz[0] * w), w - 1) # prevent the index overflow + y = min(int(pixel_xyz[1] * h), h - 1) # prevent the index overflow + if x <= 0 or x >= w or y <= 0 or y >= h: + ''' out of index ''' + return + + z = self.depth_image_filtered[y, x] + if idx == min_z_index: + offset_min_z = z + xyz_world = rs.rs2_deproject_pixel_to_point(self.depth_intrinsics, [x, y], z) + selected_keypoint_world_arr.append(xyz_world) + + s_x, s_y, s_z = self.get_scales_xyz_coordinate_to_world(selected_keypoint_arr=selected_keypoint_arr, + selected_keypoint_world_arr=selected_keypoint_world_arr, + sz_type='sx') + # obtan scale of x,y and z + # - end + ########################################################## + + # calculate world xyz of all keypoints + # There is the keypoint which the nearest keypoint from camera, we make its z-value as 0. + # cf) Originally, wrist point is 0 on mediapipe algorithm. + ''' + TODO + Apply scale of x,y and z + and offset from top distance !!! 2024.05.13 + ''' + align_z_keypoint_arr = keypoint_arr + align_z_keypoint_arr[:, 2] = keypoint_arr[:, 2] - min_z + align_z_keypoint_world_arr = [] + for idx, pixel_xyz in enumerate(align_z_keypoint_arr): + up_x = int(pixel_xyz[0] * w) + up_y = int(pixel_xyz[1] * h) + x = min(int(pixel_xyz[0] * w), w - 1) # prevent the index overflow + y = min(int(pixel_xyz[1] * h), h - 1) # prevent the index overflow + if x <= 0 or x >= w or y <= 0 or y >= h: + ''' out of index ''' + return + # z = self.depth_image_filtered[y, x] + z = pixel_xyz[2] * s_z + offset_min_z + + print(f'scale: {s_z} : pixel_xyz={pixel_xyz[2]} / z={z}') + xyz_world = rs.rs2_deproject_pixel_to_point(self.depth_intrinsics, [x, y], z) + align_z_keypoint_world_arr.append(xyz_world) + + align_z_keypoint_world_arr = np.asarray(align_z_keypoint_world_arr, dtype=np.float64) + return align_z_keypoint_world_arr + + except Exception as e: + print(f'[mediapipe_hands.py] Exception error: {e}') + finally: + pass + + def get_top_n_values(self, arr, n=3): + arr = np.array(arr) + max_indices = np.argpartition(arr, -n)[-n:] # 가장 큰 값의 인덱스를 찾습니다. + max_values = np.partition(arr, -n)[-n:] # 배열에서 가장 큰 값 3개를 찾습니다. + return max_indices, max_values + + def get_bottom_n_values(self, arr, n=3): + arr = np.array(arr) + min_indices = np.argpartition(arr, n)[:n] # 가장 작은 값의 인덱스를 찾습니다. + min_values = np.partition(arr, n)[:n] # 배열에서 가장 작은 값 3개를 찾습니다. + return min_indices, min_values + + def get_scales_xyz_coordinate_to_world(self, selected_keypoint_arr, selected_keypoint_world_arr, sz_type='sx'): + """Obtain scales of x,y and z + -- Not Used -- + scale = abs(real_distance / point_distance) + It is the average of values that have several points and are selected as a combination of two points. + (nC2) + + Args: + selected_keypoint_arr (_type_): _description_ + selected_keypoint_world_arr (_type_): _description_ + + Returns: + _type_: _description_ + """ + n_arr = np.array(selected_keypoint_arr) + w_arr = np.array(selected_keypoint_world_arr) + + scale_xyz = [] + # n = len(selected_keypoint_arr) + for idx, _ in enumerate(selected_keypoint_arr): + for i in range(idx + 1, len(selected_keypoint_arr)): + scales = abs((w_arr[idx] - w_arr[i]) / (n_arr[idx] - n_arr[i])) + scale_xyz.append(scales) + scale_xyz = np.asarray(scale_xyz) # convert to numpay array + scale_x = np.mean(scale_xyz[:, 0]) + scale_y = np.mean(scale_xyz[:, 1]) + if (sz_type == 'sx'): + scale_z = scale_x + else: + scale_z = np.mean(scale_xyz[:, 2]) + + return scale_x, scale_y, scale_z + + # def get_hand_keypoint_normalized_xyz(self, hand_index=0, keypoint='index_finger_tip'): + # try: + # keypoint_index = self.keypoints['keypoints'][keypoint] + # landmarks = self.hand_results[hand_index]['landmarks'] + # x = landmarks[keypoint_index,0] + # y = landmarks[keypoint_index,1] + # z = landmarks[keypoint_index,2] + + # return x,y,z + # except Exception as e: + # print(f'[mediapipe_hands.py] Exception error: {e}') + # finally: + # pass + + def rs_init(self, fps=30): + # Realsense 카메라 객체 생성 + self.pipeline = rs.pipeline() + self.config = rs.config() + # config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) + # config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) + width = self.image_width + height = self.image_height + self.config.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps) + self.config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps) + + # 카메라 시작 + self.profile = self.pipeline.start(self.config) + # self.align = rs.align(rs.stream.color) + + self.color_intrinsics = self.profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() + self.depth_intrinsics = self.profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() + + # color_intrinsics = align.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() + # depth_intrinsics = align.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() + + print(f'intrinsics(color): {self.color_intrinsics}') + print(f'intrinsics(depth): {self.depth_intrinsics}') + + self.depth_sensor = self.profile.get_device().first_depth_sensor() + if self.depth_sensor.supports(rs.option.depth_units): + self.depth_sensor.set_option(rs.option.depth_units, 0.001) + print(f'depth sensor: {self.depth_sensor} - change depth_units to : 0.001') + self.pipeline.stop() + print(f'Reopen the camera...') + time.sleep(1) + + self.profile = self.pipeline.start(self.config) + self.align = rs.align(rs.stream.color) + else: + print('depth sensor doesn''t support changing depth_units.') + + def get_frames(self): + frames = self.pipeline.wait_for_frames() + + # Align the depth frame to color frame + align = rs.align(rs.stream.color) + frames = align.process(frames) + + # Get aligned frames + aligned_depth_frame = frames.get_depth_frame() + color_frame = frames.get_color_frame() + + # Validate that both frames are valid + if not aligned_depth_frame or not color_frame: + raise RuntimeError("Could not acquire aligned frames.") + + ''' + TODO depth scale - D400: 0 L515: /4 + ''' + # self.depth_image = np.asanyarray(aligned_depth_frame.get_data()) / 4 + self.depth_image = np.asanyarray(aligned_depth_frame.get_data()) + self.color_image = np.asanyarray(color_frame.get_data()) + + # self.color_image = np.asanyarray(color_frame.get_data()) + # self.depth_image = np.asanyarray(depth_frame.get_data()) + + # depth_to_disparity_transform = rs.disparity_transform(True) # True for depth to disparity + # align_frames = depth_to_disparity_transform.process(align_frames) + # + # align_frames = rs.spatial_filter().process(align_frames) + # align_frames = rs.temporal_filter().process(align_frames) + # + # disparity_to_depth_transform = rs.disparity_transform(False) + # align_frames = disparity_to_depth_transform.process(align_frames).as_frameset() + + + # return + return self.color_image, self.depth_image + + def realsense_demo(self): + global g_hand_data + try: + image_width = self.image_width + image_height = self.image_height + # mphand = MediaPipeHandLandmarkDetector() + self.rs_init() + + while True: + image_color, image_depth = self.get_frames() + + self.hand_detection(image_color, image_depth, self.depth_intrinsics) + + index_finger_tip_image = self.hand_landmarks['landmarks'] + index_finger_tip_world = self.hand_landmarks['world_landmarks'] + + if index_finger_tip_image is None or index_finger_tip_world is None: + continue + xyz_idx = index_finger_tip_image[8] + x = min(int(xyz_idx[0] * image_width), image_width - 1) # prevent the index overflow + y = min(int(xyz_idx[1] * image_height), image_height - 1) # prevent the index overflow + # if x <= 0 or x >= w or y <= 0 or y >= h: + # ''' out of index ''' + # return + z = self.depth_image_filtered[y, x] + z2 = index_finger_tip_world[8][2] + + finger_depth = z + + try: + m_f = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='middle_finger_tip', coord='canonical') + m_f_z = self.depth_image_filtered[m_f[1], m_f[0]] + cv2.putText(self.color_image, f"{m_f_z:.1f}", (m_f[0], m_f[1] - 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1) + m_f = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='ring_finger_tip', coord='canonical') + m_f_z = self.depth_image_filtered[m_f[1], m_f[0]] + cv2.putText(self.color_image, f"{m_f_z:.1f}", (m_f[0], m_f[1] - 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1) + m_f = self.get_hand_keypoint_pixel_xyz(hand_index=0, keypoint='pinky_tip', coord='canonical') + m_f_z = self.depth_image_filtered[m_f[1], m_f[0]] + cv2.putText(self.color_image, f"{m_f_z:.1f}", (m_f[0], m_f[1] - 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1) + except Exception as e: + continue + pass + + + cv2.line(self.color_image, (int(image_width / 2), int(image_height / 2)), + (int(image_width / 2), int(image_height / 2)), (255, 0, 0), 5) + cv2.putText(self.color_image, f"{finger_depth:.1f} mm", + (x, y - 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1) + # cv2.putText(self.color_image, f"(scale: {z2:.1f}) mm", + # (x, y), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1) + # cv2.putText(self.color_image, + # f"{x:.1f}, {y:.1f}, {z:.1f} mm", + # (x, y + 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1) + cv2.line(self.depth_image_filtered, (x, y), (x, y), (0, 255, 0), 5) + cv2.putText(self.depth_image_filtered, f"{finger_depth:.1f} mm", + (x, y), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 0), 1) + # cv2.putText(self.depth_image_filtered, + # f"{x:.1f}, {y:.1f}, {z:.1f} mm", + # (x, y + 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 0), 1) + + + + + # if self.color_image is None or self.depth_colormap is None: + # continue + + cv2.imshow('MediaPipe Hands', self.color_image) + + # cv2.imshow('depth image', depth_show_img) + + cv2.imshow('depth image2', self.depth_colormap) + cv2.imshow('depth image3', self.blended_image) + + + + if cv2.waitKey(5) & 0xFF == 27: + break + except KeyboardInterrupt: + print(f'Keyboard Interrupt (SIGINT)') + finally: + pass + +def main(): + HLD = MediaPipeHandLandmarkDetector() + thread_mediapipe_hand = threading.Thread(target=HLD.realsense_demo) + thread_mediapipe_hand.daemon = True + thread_mediapipe_hand.start() + + print('RealTimePlot3D class is update...') + + real_time_plot = RealTimePlot3D() + real_time_plot.start() + + print('show start') + plt.show() + print('show end') + + # t = Process(target=HLD.realsense_demo(), args=(10,)) + # t.start() + # t.join() + +if __name__ == '__main__': + main() + diff --git a/common/mediapipe_api/handlandmarks/test.py b/common/mediapipe_api/handlandmarks/test.py new file mode 100644 index 0000000..64749fb --- /dev/null +++ b/common/mediapipe_api/handlandmarks/test.py @@ -0,0 +1,97 @@ +import cv2 #영상처리를 위한 라이브러리 +import numpy as np # 백터 및 행렬 연산 라이브러리 +import pyrealsense2 as rs # realsense 카메라를 파이선으로 제어 가능 모듈 +import time #시간관련 +import os #운영체제 관련 모듈 operating system +import matplotlib.pyplot as plt + + + +from RealSense_Utilities.realsense_api.realsense_api import RealSenseCamera +from RealSense_Utilities.realsense_api.realsense_api import find_realsense +from RealSense_Utilities.realsense_api.realsense_api import frame_to_np_array +from mpl_toolkits.mplot3d import Axes3D +from PIL import Image + +DEVICE = "cuda" # 그래픽카드 할당 사용 + +#realsense sdk사용하여 딥스 카메라 픽셀좌표에서 물리좌표로 변환하는 함수 x:가로 y:세로 d:깊이 intr: 카메라 내부 특성(렌즈 초점거리, 광학중심등) +def convert_depth_to_phys_coord(xp, yp, depth, intr): + result = rs.rs2_deproject_pixel_to_point(intr, [int(xp), int(yp)], depth) + + return result[0], result[1], result[2] # 반환된 xyz좌표 + +def main(): + # camera environment setup/카메라 초기화 및 설정 + frame_height, frame_width, channels = (480, 640, 3) # (이미지 픽셀 수,픽셀수 , 채널 수) + + cameras = {} # 빈 딕셔너리 생성 파이썬에서 키-쌍을 저장하는 자료구조 + realsense_device = find_realsense() + widths = [] + heights = [] + # If using several cameras, detecting camera's individual serial. + for serial, devices in realsense_device: #반복문 realsenss_device에서 시리얼 번호와 장치 정보를 가져와 초기화 후 재설정 , cameras 딕셔너리에 저장 + cameras[serial] = RealSenseCamera(device=devices, adv_mode_flag=True, device_type='d455', + color_stream_fps=30, depth_stream_fps=30, + color_stream_height=frame_height, color_stream_width=frame_width, + depth_stream_height=frame_height, depth_stream_width=frame_width, + disable_color_auto_exposure=False) + time.sleep(5) # 카메라 초기화 후 5초 대기 + + _, rs_main = cameras.popitem() # 마지막 키 값을 제거 후 반환 rs_main에 저장 , 하나의 realsense 카메라를 사용하기 위함 + + if rs_main is None: + print("can't initialize realsense cameras") + + # main streaming part + while True: #while 루프문으 사용하여 실시간 프레임 캡쳐 및 처리 + try: + # To get real-time frame, using get_data() function to capturing frame. 실시간 프레임 캡쳐 + rs_main.get_data() + + # captured every frameset includes RGB frame and depth frame. + frameset = rs_main.frameset + + # required to align depth frame to RGB frame. + rs_main.get_aligned_frames(frameset, aligned_to_color=True) + + # applying filters in depth frame. + frameset = rs_main.depth_to_disparity.process(rs_main.frameset) + frameset = rs_main.spatial_filter.process(frameset) + frameset = rs_main.temporal_filter.process(frameset) + frameset = rs_main.disparity_to_depth.process(frameset) + frameset = rs_main.hole_filling_filter.process(frameset).as_frameset() + + # It is recommended to use a copy of the RGB image frame. + img_rs0 = np.copy(frame_to_np_array(frameset.get_color_frame())) + + # Same to depth frame. + img_depth = np.copy(frame_to_np_array(frameset.get_depth_frame())) + img_raw = np.copy(img_rs0) + + results = model.predict(img_rs0) + im_array = results[0].plot() + + # use opencv to visualize results. + resized_image = cv2.resize(im_array, dsize=(0, 0), fx=1, fy=1, interpolation=cv2.INTER_AREA) + cv2.namedWindow('RealSense_front', cv2.WINDOW_NORMAL) + cv2.resizeWindow('RealSense_front', resized_image.shape[1], resized_image.shape[0]) + cv2.imshow('RealSense_front', resized_image) + + + key = cv2.pollKey() + + if key & 0xFF == ord('q') or key == 27: + cv2.destroyAllWindows() + break + + except RuntimeError as runexpt: + print(runexpt, " frame skipped") + continue + + rs_main.stop() + print("main process closed") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/common/mediapipe_api/handlandmarks/wja1.png b/common/mediapipe_api/handlandmarks/wja1.png new file mode 100644 index 0000000..d2f9119 Binary files /dev/null and b/common/mediapipe_api/handlandmarks/wja1.png differ diff --git a/common/mediapipe_api/handlandmarks/yj.jpg b/common/mediapipe_api/handlandmarks/yj.jpg new file mode 100644 index 0000000..522c8b9 Binary files /dev/null and b/common/mediapipe_api/handlandmarks/yj.jpg differ diff --git a/scripts/__init__.py b/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/__pycache__/handlandmarks_show_plot.cpython-38.pyc b/scripts/__pycache__/handlandmarks_show_plot.cpython-38.pyc new file mode 100644 index 0000000..47b5c17 Binary files /dev/null and b/scripts/__pycache__/handlandmarks_show_plot.cpython-38.pyc differ diff --git a/scripts/__pycache__/handlandmarks_show_plot_legacy.cpython-38.pyc b/scripts/__pycache__/handlandmarks_show_plot_legacy.cpython-38.pyc new file mode 100644 index 0000000..21c9828 Binary files /dev/null and b/scripts/__pycache__/handlandmarks_show_plot_legacy.cpython-38.pyc differ diff --git a/scripts/__pycache__/handlandmarks_with_realsense.cpython-38.pyc b/scripts/__pycache__/handlandmarks_with_realsense.cpython-38.pyc new file mode 100644 index 0000000..820140a Binary files /dev/null and b/scripts/__pycache__/handlandmarks_with_realsense.cpython-38.pyc differ diff --git a/scripts/__pycache__/tcp_server.cpython-38.pyc b/scripts/__pycache__/tcp_server.cpython-38.pyc new file mode 100644 index 0000000..72b1fc7 Binary files /dev/null and b/scripts/__pycache__/tcp_server.cpython-38.pyc differ diff --git a/scripts/handlandmarks_show_plot.py b/scripts/handlandmarks_show_plot.py new file mode 100644 index 0000000..f321ad2 --- /dev/null +++ b/scripts/handlandmarks_show_plot.py @@ -0,0 +1,111 @@ +import matplotlib +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import multiprocessing as mp +from quick_queue import QQueue +import threading +import time +import sys, os + +class RealTimePlot(): + def __init__(self, queue_handpose, queue_points, num_points=21): + self.num_points = num_points + self.queue_handpose = queue_handpose + self.queue_points = queue_points + self.handpose = None + self.hand_points = np.zeros((21, 3)) + + self.fig = plt.figure() + + self.ax = self.fig.add_subplot(131, projection='3d') + self.scatter = self.ax.scatter([], [], []) + + self.ax_cp = self.fig.add_subplot(132, projection='3d') + self.scatter_world_points = self.ax_cp.scatter([], [], []) + + self.ax_wp = self.fig.add_subplot(133, projection='3d') + self.scatter_canonical_points = self.ax_wp.scatter([], [], []) + + self.ani = FuncAnimation(self.fig, self.update_plot, frames=1, interval=10) + self.count = 0 + self.s_t = time.time() + self.pps = 0 + + def update_plot(self, frame): + if self.count == 1: # start + self.s_t = time.time() + try: + # if self.queue_handpose.full(): + self.handpose = self.queue_handpose.get() + self.hand_points = self.queue_points.get() + # print(f"[Process-3D plot] handpose : {self.handpose}") + except Exception as e: + return + + self.pps = self.count / (time.time() - self.s_t) + self.count += 1 + + if self.handpose['landmarks'] is None or self.hand_points is None: + return None + + self.ax.cla() + self.ax_cp.cla() + self.ax_wp.cla() + + self.ax.set_title('normalized') + self.ax.set_xlabel('X Label') + self.ax.set_ylabel('Y Label') + self.ax.set_zlabel('Z Label') + # self.ax.set_xlim(-1, 1) + # self.ax.set_ylim(-1, 1) + # self.ax.set_zlim(-1, 1) + + self.ax_cp.set_title('canonical') + self.ax_cp.set_xlabel('X Label') + self.ax_cp.set_ylabel('Y Label') + self.ax_cp.set_zlabel('Z Label') + self.ax_cp.set_xlim(0, 640) + self.ax_cp.set_ylim(0, 480) + self.ax_cp.set_zlim(200, 700) + + self.ax_wp.set_title('world coordinate') + self.ax_wp.set_xlabel('X Label') + self.ax_wp.set_ylabel('Y Label') + self.ax_wp.set_zlabel('Z Label') + self.ax_wp.set_xlim(-300, 300) + self.ax_wp.set_ylim(-300, 300) + self.ax_wp.set_zlim(200, 700) + + colors = ['black', 'blue', 'green', 'orange', 'red', 'black'] + intervals = [4, 8, 12, 16, 20] + + l_p = self.handpose['landmarks'] + w_p = self.handpose['world_landmarks'] + c_p = self.hand_points + + self.scatter = self.ax.scatter(l_p[:, 0], l_p[:, 1], l_p[:, 2], color='black', s=5, alpha=1) + self.scatter_canonical_points = self.ax_cp.scatter(c_p[:, 0], c_p[:, 1], c_p[:, 2], color='black', s=5, alpha=1) + self.scatter_world_points = self.ax_wp.scatter(w_p[:, 0], w_p[:, 1], w_p[:, 2], color='black', s=5, alpha=1) + + for i in range(len(intervals)): + start_idx = 0 if i == 0 else intervals[i - 1] + 1 + end_idx = intervals[i] + self.ax.plot(l_p[start_idx:end_idx + 1, 0], l_p[start_idx:end_idx + 1, 1], l_p[start_idx:end_idx + 1, 2], color=colors[i]) + self.ax_cp.plot(c_p[start_idx:end_idx + 1, 0], c_p[start_idx:end_idx + 1, 1], c_p[start_idx:end_idx + 1, 2], color='blue') + self.ax_wp.plot(w_p[start_idx:end_idx + 1, 0], w_p[start_idx:end_idx + 1, 1], w_p[start_idx:end_idx + 1, 2], color='red') + + def plot_show(self): + plt.show() + +def start_real_time_plot(queue_handpose, queue_points): + real_time_plot = RealTimePlot(queue_handpose, queue_points) + real_time_plot.plot_show() + +if __name__ == '__main__': + queue_handpose = mp.Queue() + queue_points = mp.Queue() + plot_process = mp.Process(target=start_real_time_plot, args=(queue_handpose, queue_points)) + plot_process.start() + plot_process.join() diff --git a/scripts/handlandmarks_show_plot_legacy.py b/scripts/handlandmarks_show_plot_legacy.py new file mode 100644 index 0000000..0f56c1a --- /dev/null +++ b/scripts/handlandmarks_show_plot_legacy.py @@ -0,0 +1,117 @@ +import matplotlib +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import multiprocessing as mp +from quick_queue import QQueue +import threading +import time +import sys, os + +class RealTimePlot(): + def __init__(self, queue_handpose, queue_points, num_points=21): + self.num_points = num_points + self.queue_handpose = queue_handpose + self.queue_points = queue_points + + self.fig = plt.figure() + + self.ax = self.fig.add_subplot(231, projection='3d') + self.scatter = self.ax.scatter([], [], []) + + self.ax_cp = self.fig.add_subplot(232, projection='3d') + self.scatter_world_points = self.ax_cp.scatter([], [], []) + + self.ax_wp = self.fig.add_subplot(233, projection='3d') + self.scatter_canonical_points = self.ax_wp.scatter([], [], []) + + self.ani = FuncAnimation(self.fig, self.update_plot, frames=1, interval=10) + self.count = 0 + self.s_t = time.time() + self.pps = 0 + + def update_plot(self, frame): + if self.count == 1: # start + self.s_t = time.time() + try: + # queue_handpose = self.queue_handpose.get_nowait() + # queue_points = self.queue_points.get_nowait() + queue_handpose = self.queue_handpose.get() + queue_points = self.queue_points.get() + except (mp.queues.Empty): + # print(f'{mp.queues.Empty} queue empty') + return + + # print(f'{queue_handpose}') + # print(f'=============== Queue.get() ============================') + # print(f'count = {self.count}') + # print(f'[3D Plot Node] {time.time()} : {self.handpose}') + # print(f'[Realsense Node] {time.time()} : {queue_points}') + print(f'[3D plot] pps = {self.count / (time.time() - self.s_t)}') + # print(f'========================================================') + self.pps = self.count / (time.time() - self.s_t) + self.count += 1 + + # self.fig.text(0.95, 0.95, f'FPS: {self.pps:.2f}', ha='right', va='top', + # fontsize=12) + + if queue_handpose['landmarks'] is None or queue_points is None: + return + + self.ax.cla() + self.ax_cp.cla() + self.ax_wp.cla() + + self.ax.set_xlabel('X Label') + self.ax.set_ylabel('Y Label') + self.ax.set_zlabel('Z Label') + # self.ax.set_xlim(-1, 1) + # self.ax.set_ylim(-1, 1) + # self.ax.set_zlim(-1, 1) + + self.ax_cp.set_xlabel('X Label') + self.ax_cp.set_ylabel('Y Label') + self.ax_cp.set_zlabel('Z Label') + self.ax_cp.set_xlim(300, 500) + self.ax_cp.set_ylim(100, 300) + self.ax_cp.set_zlim(400, 700) + + self.ax_wp.set_xlabel('X Label') + self.ax_wp.set_ylabel('Y Label') + self.ax_wp.set_zlabel('Z Label') + self.ax_wp.set_xlim(300, 500) + self.ax_wp.set_ylim(100, 300) + self.ax_wp.set_zlim(400, 700) + + colors = ['black', 'blue', 'green', 'orange', 'red', 'black'] + intervals = [4, 8, 12, 16, 20] + + l_p = queue_handpose['landmarks'] + w_p = queue_handpose['world_landmarks'] + c_p = queue_points + self.scatter = self.ax.scatter(l_p[:, 0], l_p[:, 1], l_p[:, 2], color='black', s=5, alpha=1) + self.scatter_canonical_points = self.ax_cp.scatter(c_p[:, 0], c_p[:, 1], c_p[:, 2], color='black', s=5, alpha=1) + self.scatter_world_points = self.ax_wp.scatter(w_p[:, 0], w_p[:, 1], w_p[:, 2], color='black', s=5, alpha=1) + + for i in range(len(intervals)): + start_idx = 0 if i == 0 else intervals[i - 1] + 1 + end_idx = intervals[i] + self.ax.plot(l_p[start_idx:end_idx + 1, 0], l_p[start_idx:end_idx + 1, 1], l_p[start_idx:end_idx + 1, 2], color=colors[i]) + self.ax_cp.plot(c_p[start_idx:end_idx + 1, 0], c_p[start_idx:end_idx + 1, 1], c_p[start_idx:end_idx + 1, 2], color='blue') + self.ax_wp.plot(w_p[start_idx:end_idx + 1, 0], w_p[start_idx:end_idx + 1, 1], w_p[start_idx:end_idx + 1, 2], color='red') + + return + def plot_show(self): + plt.show() + +def start_real_time_plot(queue_handpose, queue_points): + real_time_plot = RealTimePlot(queue_handpose, queue_points) + real_time_plot.plot_show() + +if __name__ == '__main__': + queue_handpose = mp.Queue() + queue_points = mp.Queue() + plot_process = mp.Process(target=start_real_time_plot, args=(queue_handpose, queue_points)) + plot_process.start() + plot_process.join() \ No newline at end of file diff --git a/scripts/handlandmarks_with_realsense.py b/scripts/handlandmarks_with_realsense.py new file mode 100644 index 0000000..e3208e5 --- /dev/null +++ b/scripts/handlandmarks_with_realsense.py @@ -0,0 +1,127 @@ +import queue + +import numpy as np +import sys +import os +import time +import cv2 +import multiprocessing as mp +from quick_queue import QQueue +import signal + +sys.path.append(os.path.join(os.path.dirname(__file__), '../common')) +from mediapipe_api.handlandmarks import handlandmarks +from RealSense_Utilities.realsense_api.realsense_api import RealSenseCamera +from RealSense_Utilities.realsense_api.realsense_api import find_realsense +from RealSense_Utilities.realsense_api.realsense_api import frame_to_np_array + + +def handlandmarks_with_realsense(queue_handpose, queue_points, queue_handpose_sub, queue_points_sub): + hld = handlandmarks.MediaPipeHandLandmarkDetector(replace_threshold=50) + + frame_height, frame_width, channels = (480, 640, 3) # (이미지 픽셀 수,픽셀수 , 채널 수) + cameras = {} # 빈 딕셔너리 생성 파이썬에서 키-쌍을 저장하는 자료구조 + realsense_device = find_realsense() + # If using several cameras, detecting camera's individual serial. + for serial, devices in realsense_device: #반복문 realsenss_device에서 시리얼 번호와 장치 정보를 가져와 초기화 후 재설정 , cameras 딕셔너리에 저장 + cameras[serial] = RealSenseCamera(device=devices, adv_mode_flag=True, device_type='d455', + color_stream_fps=30, depth_stream_fps=30, + color_stream_height=frame_height, color_stream_width=frame_width, + depth_stream_height=frame_height, depth_stream_width=frame_width, + disable_color_auto_exposure=False) + time.sleep(1) # 카메라 초기화 후 대기 + + _, rs_main_camera = cameras.popitem() # 마지막 키 값을 제거 후 반환 rs_main에 저장 , 하나의 realsense 카메라를 사용하기 위함 + print(f"{rs_main_camera}") + + count = 0 + s_t = time.time() + try: + while True: + try: + rs_main_camera.get_data() + # captured every frameset includes RGB frame and depth frame. + frameset = rs_main_camera.frameset + + # required to align depth frame to RGB frame. + rs_main_camera.get_aligned_frames(frameset, aligned_to_color=True) + + # applying filters in depth frame. + frameset = rs_main_camera.depth_to_disparity.process(rs_main_camera.frameset) + frameset = rs_main_camera.spatial_filter.process(frameset) + frameset = rs_main_camera.temporal_filter.process(frameset) + frameset = rs_main_camera.disparity_to_depth.process(frameset) + frameset = rs_main_camera.hole_filling_filter.process(frameset) + frameset = frameset.as_frameset() + + # It is recommended to use a copy of the RGB image frame. + img_color = np.copy(frame_to_np_array(frameset.get_color_frame())) + img_depth = np.copy(frame_to_np_array(frameset.get_depth_frame())) + img_depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(img_depth, alpha=0.3), cv2.COLORMAP_JET) + + hld.hand_detection(img_color, img_depth, rs_main_camera.depth_intrinsics) + + empty = queue_handpose.empty() + if queue_handpose.empty(): + queue_handpose.put(hld.hand_pose) + queue_points.put(hld.canonical_points) + empty = queue_handpose_sub.empty() + if queue_handpose_sub.empty(): + queue_handpose_sub.put(hld.hand_pose) + queue_points_sub.put(hld.canonical_points) + + # queue_handpose.put(hld.hand_pose) + # queue_points.put(hld.canonical_points) + # queue_handpose_sub.put(hld.hand_pose) + # queue_points_sub.put(hld.canonical_points) + + count += 1 + # all_img = np.hstack([hld.drawing_image, img_color, img_depth_colormap]) + all_img = np.hstack([hld.drawing_image, hld.drawing_depth_image]) + + # use opencv to visualize results. + if hld.drawing_image is not None: + # cv2.imshow('RealSense_front', hld.drawing_image) + cv2.imshow('RealSense_front', all_img) + + key = cv2.pollKey() + if key & 0xFF == ord('q') or key == 27: + break + + # if cv2.waitKey(1) == ord('q'): + # break + + except RuntimeError as runexpt: + print(runexpt, " frame skipped") + continue + finally: + cv2.destroyAllWindows() + rs_main_camera.stop() + print("main process closed") + +def signal_handler(sig, frame): + print('Signal received, terminating process...') + if hand_process.is_alive(): + hand_process.terminate() + hand_process.join() + sys.exit(1) + +if __name__ == '__main__': + queue1 = mp.Manager().Queue(maxsize=1) + queue2 = mp.Manager().Queue(maxsize=1) + queue3 = mp.Manager().Queue(maxsize=1) + queue4 = mp.Manager().Queue(maxsize=1) + hand_process = mp.Process(target=handlandmarks_with_realsense, args=(queue1, queue2, queue3, queue4)) + hand_process.start() + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + try: + hand_process.join() + except KeyboardInterrupt: + hand_process.terminate() + hand_process.join() + print(f'hand landmark with realsense Process terminated') + + diff --git a/scripts/main.py b/scripts/main.py new file mode 100644 index 0000000..e6d3eb1 --- /dev/null +++ b/scripts/main.py @@ -0,0 +1,25 @@ +import multiprocessing as mp +from handlandmarks_show_plot import start_real_time_plot +from handlandmarks_with_realsense import handlandmarks_with_realsense +from tcp_server import start_tcp_server + +if __name__ == '__main__': + queue_handpose = mp.Manager().Queue(maxsize=1) + queue_points = mp.Manager().Queue(maxsize=1) + queue_handpose_sub = mp.Manager().Queue(maxsize=1) + queue_points_sub = mp.Manager().Queue(maxsize=1) + # queue_handpose = mp.Queue(maxsize=1) + # queue_points = mp.Queue(maxsize=1) + + data_process = mp.Process(target=handlandmarks_with_realsense, args=(queue_handpose, queue_points, queue_handpose_sub, queue_points_sub)) + data_process.start() + + plot_process = mp.Process(target=start_real_time_plot, args=(queue_handpose_sub, queue_points_sub)) + plot_process.start() + + tcp_process = mp.Process(target=start_tcp_server, args=(queue_handpose, queue_points)) + tcp_process.start() + + data_process.join() + plot_process.join() + tcp_process.join() diff --git a/scripts/tcp_client.py b/scripts/tcp_client.py new file mode 100644 index 0000000..c21d50b --- /dev/null +++ b/scripts/tcp_client.py @@ -0,0 +1,76 @@ +import socket +import json +import threading +import struct +import numpy as np + +class TCPClient: + def __init__(self): + self.config = self.load_config('tcp_config.json') + self.host = self.config['host'] + self.port = self.config['port'] + self.buffer_size = self.config['buffer_size'] + self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.hand_points = None + + def load_config(self, config_file): + with open(config_file, 'r') as file: + return json.load(file) + + def connect(self): + try: + self.client_socket.connect((self.host, self.port)) + print(f"Connected to server at {self.host}:{self.port}") + threading.Thread(target=self.thread_receive_data, daemon=True).start() + except Exception as e: + print(f"Failed to connect to server: {e}") + + def thread_receive_data(self): + try: + while True: + self.client_socket.sendall(b'GET_HAND') + + data = self.client_socket.recv(self.buffer_size) + print(f'data (byte={len(data)}: {data}') + if not data: + continue + + if data.startswith(b'NONE'): + # print("No hands") + continue + + if len(data) != self.buffer_size: + print(f"Received data of unexpected size: {len(data)} (must be {self.buffer_size})") + continue + # handpoint : (21,3) array -> 63 points -> (63 * 4 = 252) bytes + points_num = 63 + points_byte_len = 252 + unpacked_data = struct.unpack(f'!{points_num}f', data[:points_byte_len]) + hand_points = np.array(unpacked_data).reshape((21, 3)) + print(f"Received data[hand points]: {hand_points}") + + except Exception as e: + print(f"Exception in receive_data: {e}") + finally: + self.close() + + def send_data(self, data): + try: + self.client_socket.sendall(b'GET_HAND') + except Exception as e: + print(f"Exception in send_data: {e}") + + def close(self): + self.client_socket.close() + print(f'client[{self.client_socket}] closed') + +if __name__ == "__main__": + client = TCPClient() + client.connect() + print(f'TCP client on.') + while True: + try: + pass + except KeyboardInterrupt: + client.close() + diff --git a/scripts/tcp_client2.py b/scripts/tcp_client2.py new file mode 100644 index 0000000..bd05a80 --- /dev/null +++ b/scripts/tcp_client2.py @@ -0,0 +1,48 @@ +import socket +import json +import threading + +class TCPClient: + def __init__(self): + self.config = self.load_config('tcp_config.json') + self.host = self.config['host'] + self.port = self.config['port'] + self.buffer_size = self.config['buffer_size'] + self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + + def load_config(self, config_file): + with open(config_file, 'r') as file: + return json.load(file) + + def connect(self): + try: + self.client_socket.connect((self.host, self.port)) + print(f"Connected to server at {self.host}:{self.port}") + threading.Thread(target=self.receive_data, daemon=True).start() + except Exception as e: + print(f"Failed to connect to server: {e}") + + def receive_data(self): + try: + while True: + data = self.client_socket.recv(self.buffer_size) + if not data: + break + print(f"Received data: {data.decode('utf-8')}") + except Exception as e: + print(f"Exception in receive_data: {e}") + finally: + self.client_socket.close() + + def send_data(self, data): + try: + self.client_socket.sendall(data.encode('utf-8')) + except Exception as e: + print(f"Exception in send_data: {e}") + +if __name__ == "__main__": + client = TCPClient() + client.connect() + while True: + message = input("Enter message to send: ") + client.send_data(message) diff --git a/scripts/tcp_client3.py b/scripts/tcp_client3.py new file mode 100644 index 0000000..bd05a80 --- /dev/null +++ b/scripts/tcp_client3.py @@ -0,0 +1,48 @@ +import socket +import json +import threading + +class TCPClient: + def __init__(self): + self.config = self.load_config('tcp_config.json') + self.host = self.config['host'] + self.port = self.config['port'] + self.buffer_size = self.config['buffer_size'] + self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + + def load_config(self, config_file): + with open(config_file, 'r') as file: + return json.load(file) + + def connect(self): + try: + self.client_socket.connect((self.host, self.port)) + print(f"Connected to server at {self.host}:{self.port}") + threading.Thread(target=self.receive_data, daemon=True).start() + except Exception as e: + print(f"Failed to connect to server: {e}") + + def receive_data(self): + try: + while True: + data = self.client_socket.recv(self.buffer_size) + if not data: + break + print(f"Received data: {data.decode('utf-8')}") + except Exception as e: + print(f"Exception in receive_data: {e}") + finally: + self.client_socket.close() + + def send_data(self, data): + try: + self.client_socket.sendall(data.encode('utf-8')) + except Exception as e: + print(f"Exception in send_data: {e}") + +if __name__ == "__main__": + client = TCPClient() + client.connect() + while True: + message = input("Enter message to send: ") + client.send_data(message) diff --git a/scripts/tcp_config.json b/scripts/tcp_config.json new file mode 100644 index 0000000..021aff4 --- /dev/null +++ b/scripts/tcp_config.json @@ -0,0 +1,5 @@ +{ + "host": "127.0.0.1", + "port": 7777, + "buffer_size": 512 +} diff --git a/scripts/tcp_server.py b/scripts/tcp_server.py new file mode 100644 index 0000000..082902f --- /dev/null +++ b/scripts/tcp_server.py @@ -0,0 +1,114 @@ +import socket +import multiprocessing as mp +import threading +import json +import time +import numpy as np +import struct + +class TCPServer: + def __init__(self, queue_handpose, queue_points): + self.queue_handpose = queue_handpose + self.queue_points = queue_points + self.handpose = None + self.hand_points = np.zeros((21, 3)) + self.config = self.load_config('tcp_config.json') + self.host = self.config['host'] + self.port = self.config['port'] + self.buffer_size = self.config['buffer_size'] + self.lock = threading.Lock() + self.client = None + + self.pps = 0 + self.count = 0 + self.s_t = time.time() + + def load_config(self, config_file): + with open(config_file, 'r') as file: + return json.load(file) + + def handle_client(self): + print(f"[*] Handling client {self.client.getpeername()}") + try: + while True: + # request_flag = False + request_flag = self.client_callback() + if request_flag: + self.get_hand_landmarks() + self.broadcast(self.client) + else: + break + + time.sleep(0.01) + except Exception as e: + print(f"Exception in handle_client: {e}") + finally: + print(f"[*] Client {self.client.getpeername()} disconnected.") + self.client.close() + + def client_callback(self): + request = self.client.recv(self.buffer_size) + if not request: + # self.client = None + return False + + if request.decode('utf-8') == 'GET_HAND': + return True + + + def broadcast(self, client_socket): + if self.handpose is None or self.handpose['index'] is None: + data = b'NONE' + b'\0' * (self.buffer_size - len('NONE')) + serialized_data = data + else: + world_landmarks = self.handpose['world_landmarks'] + data_flat = world_landmarks.flatten() + serialized_data = struct.pack(f'!{data_flat.size}f', *data_flat) + + padding_size = self.buffer_size - len(serialized_data) + serialized_data = serialized_data + b'\0' * padding_size + + print(f'serialized_data (byte={len(serialized_data)}): {serialized_data}') + + try: + client_socket.sendall(serialized_data) + except Exception as e: + print(f"Exception in broadcast: {e}") + raise + + def get_hand_landmarks(self): + try: + self.handpose = self.queue_handpose.get_nowait() + self.hand_points = self.queue_points.get_nowait() + except mp.queues.Empty: + print('Queue is empty') + return + + def start_server(self): + server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + server.bind((self.host, self.port)) + server.listen(1) # 하나의 연결만 처리 + + try: + while True: + print(f"[*] Listening on {self.host}:{self.port}") + client_socket, addr = server.accept() + self.client = client_socket + print(f"[*] Accepted connection from {addr[0]}:{addr[1]}") + self.handle_client() + except KeyboardInterrupt: + print("Server shutting down.") + finally: + server.close() + +def start_tcp_server(queue_handpose, queue_points): + tcp_server = TCPServer(queue_handpose, queue_points) + tcp_server.start_server() + +if __name__ == "__main__": + queue_handpose = mp.Queue() + queue_points = mp.Queue() + tcp_process = mp.Process(target=start_tcp_server, args=(queue_handpose, queue_points)) + tcp_process.start() + tcp_process.join() diff --git a/src/custom_interfaces/CMakeLists.txt b/src/custom_interfaces/CMakeLists.txt new file mode 100644 index 0000000..4ffc82b --- /dev/null +++ b/src/custom_interfaces/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.5) +project(custom_interfaces) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +set(msg_files + "msg/HandLandmarks.msg" + "msg/HandLandmarksMultiArray.msg" +) + +set(srv_files + # file +) + +set(action_files + # file +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + # DEPENDENCIES builtin_interfaces + DEPENDENCIES builtin_interfaces std_msgs geometry_msgs sensor_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/custom_interfaces/msg/HandLandmarks.msg b/src/custom_interfaces/msg/HandLandmarks.msg new file mode 100644 index 0000000..55d9c58 --- /dev/null +++ b/src/custom_interfaces/msg/HandLandmarks.msg @@ -0,0 +1,7 @@ +# Motor operating target values +# builtin_interfaces/Time stamp +# std_msgs/Header header +uint8 index +string label +float32 score +std_msgs/Float64MultiArray landmarks \ No newline at end of file diff --git a/src/custom_interfaces/msg/HandLandmarksMultiArray.msg b/src/custom_interfaces/msg/HandLandmarksMultiArray.msg new file mode 100644 index 0000000..b337140 --- /dev/null +++ b/src/custom_interfaces/msg/HandLandmarksMultiArray.msg @@ -0,0 +1,4 @@ +# Motor operating target values +# builtin_interfaces/Time stamp +std_msgs/Header header +custom_interfaces/HandLandmarks[] hands \ No newline at end of file diff --git a/src/custom_interfaces/package.xml b/src/custom_interfaces/package.xml new file mode 100644 index 0000000..8a074d7 --- /dev/null +++ b/src/custom_interfaces/package.xml @@ -0,0 +1,27 @@ + + + + custom_interfaces + 0.0.0 + TODO: Package description + daeyun + Apache-2.0 + + ament_cmake + rosidl_default_generators + + std_msgs + geometry_msgs + sensor_msgs + + builtin_interfaces + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/handpose_pkg/handpose_pkg/__pycache__/handpose3Dshow.cpython-38.pyc b/src/handpose_pkg/handpose_pkg/__pycache__/handpose3Dshow.cpython-38.pyc index f5e7e7c..eb9d4ba 100644 Binary files a/src/handpose_pkg/handpose_pkg/__pycache__/handpose3Dshow.cpython-38.pyc and b/src/handpose_pkg/handpose_pkg/__pycache__/handpose3Dshow.cpython-38.pyc differ diff --git a/src/handpose_pkg/handpose_pkg/__pycache__/handpose_indexfinger_publisher.cpython-38.pyc b/src/handpose_pkg/handpose_pkg/__pycache__/handpose_indexfinger_publisher.cpython-38.pyc index d63914f..8f9cf74 100644 Binary files a/src/handpose_pkg/handpose_pkg/__pycache__/handpose_indexfinger_publisher.cpython-38.pyc and b/src/handpose_pkg/handpose_pkg/__pycache__/handpose_indexfinger_publisher.cpython-38.pyc differ diff --git a/src/handpose_pkg/handpose_pkg/demo.py b/src/handpose_pkg/handpose_pkg/demo.py new file mode 100644 index 0000000..e2c04fa --- /dev/null +++ b/src/handpose_pkg/handpose_pkg/demo.py @@ -0,0 +1,32 @@ +def handpose_extractor(): + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/handpose_pkg/handpose_pkg/realsense_demo_coordinate_v2.py b/src/handpose_pkg/handpose_pkg/realsense_demo_coordinate_v2.py deleted file mode 100644 index 7be9959..0000000 --- a/src/handpose_pkg/handpose_pkg/realsense_demo_coordinate_v2.py +++ /dev/null @@ -1,328 +0,0 @@ -import mediapipe as mp -import cv2 -import numpy as np -import uuid -import os -import pyrealsense2 as rs -import time -import matplotlib -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -from matplotlib.animation import FuncAnimation -from functools import partial -import threading -# matplotlib.use('Agg') - - -import queue -queue = queue.Queue() - -g_hand_data = np.zeros((21, 3)) -g_hand_world_data = np.zeros((21, 3)) -g_hand_world_data_normalized = np.zeros((21, 3)) - -class RealTimePlot3D: - def __init__(self, num_points=21): - self.num_points = num_points - self.data = np.random.randn(self.num_points, 3) - self.fig = plt.figure() - self.ax = self.fig.add_subplot(111, projection='3d') - self.ax.set_xlabel('X Label') - self.ax.set_ylabel('Y Label') - self.ax.set_zlabel('Z Label') - self.ax.set_xlim(-0.1, 0.1) - self.ax.set_ylim(-0.1, 0.1) - self.ax.set_zlim(-0.1, 0.1) - # self.ani = None - self.ani = FuncAnimation(self.fig, self.update_data, frames=1, interval=100) - self.scatter = self.ax.scatter(g_hand_data[:, 0], g_hand_data[:, 1], g_hand_data[:, 2]) - self.update_thread = threading.Thread(target=self.update_data) - self.update_thread.daemon = True - - def update_data(self, frames): - - global g_hand_data, g_hand_data_normalized - colors = ['black', 'blue', 'green', 'orange', 'red', 'black'] - intervals = [4, 8, 12, 16, 20] - - while True: - # 현재 점 삭제 - self.scatter.remove() - self.ax.cla() - self.ax.set_xlabel('X Label') - self.ax.set_ylabel('Y Label') - self.ax.set_zlabel('Z Label') - data = g_hand_data - - self.scatter = self.ax.scatter(data[:, 0], data[:, 1], data[:, 2], - color='black', - s=50, - alpha=1) - for i in range(len(intervals)): - start_idx = 0 if i == 0 else intervals[i - 1] + 1 - end_idx = intervals[i] - self.ax.plot(data[start_idx:end_idx + 1, 0], - data[start_idx:end_idx + 1, 1], - data[start_idx:end_idx + 1, 2], - color=colors[i]) - - - - return self.scatter - - def start(self): - # 데이터 업데이트 쓰레드 시작 - self.update_thread.start() - self.ani = FuncAnimation(self.fig, self.update_data, frames=100, interval=30) - - -class MediaPipeHandLandmarkDetector: - pass -def mediapipe_hand_detection(): - global g_hand_data, g_hand_data_normalized - hand_data = np.zeros((21, 3)) - - mp_drawing = mp.solutions.drawing_utils - mp_hands = mp.solutions.hands - - ## for filter - - # moving average - num_of_avg = 10 - finger_depth = np.zeros(num_of_avg) - - # low pass - finger_depth_prev = 0 - finger_depth_curr = 0 - filter_sensitivity = 0.3 - - # Realsense 카메라 객체 생성 - pipeline = rs.pipeline() - config = rs.config() - # config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) - # config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) - config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) - config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) - - - # 카메라 시작 - profile = pipeline.start(config) - align = rs.align(rs.stream.color) - # align = rs.align(rs.stream.depth) - - color_intrinsics = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() - depth_intrinsics = profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() - - # color_intrinsics = align.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() - # depth_intrinsics = align.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() - - print(f'intrinsics(color): {color_intrinsics}') - print(f'intrinsics(depth): {depth_intrinsics}') - - depth_sensor = profile.get_device().first_depth_sensor() - if depth_sensor.supports(rs.option.depth_units): - depth_sensor.set_option(rs.option.depth_units, 0.001) - print(f'depth sensor: {depth_sensor} - change depth_units to : 0.001') - pipeline.stop() - print(f'Reopen the camera...') - time.sleep(1) - - profile = pipeline.start(config) - align = rs.align(rs.stream.color) - else: - print('depth sensor doesn''t support changing depth_units.') - - p_time = time.time() - - with mp_hands.Hands( - min_detection_confidence=0.8, - min_tracking_confidence=0.7) as hands: - while True: - - frames = pipeline.wait_for_frames() - # align_frames = align.process(frames) - align_frames = frames - color_frame = align_frames.get_color_frame() - depth_frame = align_frames.get_depth_frame() - - # intrinsics = depth_frame.proflie.as_video_stream_profile().intrinsics - # # 내부 파라미터 출력 - # print("Width:", intrinsics.width) - # print("Height:", intrinsics.height) - # print("Principal Point [X, Y]:", intrinsics.ppx, intrinsics.ppy) - # print("Focal Length [X, Y]:", intrinsics.fx, intrinsics.fy) - # print("Distortion Model:", intrinsics.model) - - # depth_frame_spatial_filter = rs.spatial_filter().process(depth_frame) - # depth_frame_temporal_filter = rs.temporal_filter().process(depth_frame) - depth_frame = rs.hole_filling_filter().process(depth_frame) - - - if not depth_frame or not color_frame: - continue - - color_image = np.asanyarray(color_frame.get_data()) - depth_image = np.asanyarray(depth_frame.get_data()) - depth_image_origin = depth_image - # depth_image_spatial_filter = np.asanyarray(depth_frame_spatial_filter.get_data()) - # depth_image_temporal_filter = np.asanyarray(depth_frame_temporal_filter.get_data()) - - cv2.imshow('MediaPipe Hands', color_image) - cv2.imshow('MediaPipe Hands', depth_image) - - ###################################################### - # choose image with filter - depth_image = depth_image - ###################################################### - - # depth_image_origin = cv2.flip(depth_image_origin, 1) - depth_image = cv2.flip(depth_image, 1) - - # Flip the image horizontally for a later selfie-view display, and convert - # the BGR image to RGB. - image = cv2.cvtColor(cv2.flip(color_image, 1), cv2.COLOR_BGR2RGB) - depth_norm_image = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.3), cv2.COLORMAP_JET) - - - # To improve performance, optionally mark the image as not writeable to - # pass by reference. - image.flags.writeable = False - results = hands.process(image) - image_height, image_width, _ = image.shape - # Draw the hand annotations on the image. - image.flags.writeable = True - image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) - - - hand_label = None - # class of hands (detected) - if results.multi_handedness: - for handedness in results.multi_handedness: - for ids, classification in enumerate(handedness.classification): - index = classification.index - score = classification.score - label = classification.label - hand_label = label - - if hand_label == "Right": - # print("Right Hand") - if results.multi_hand_landmarks: - for hand_landmarks in results.multi_hand_landmarks: - # Here is How to Get All the Coordinates - # print(f"{hand_landmarks.categories.index} / score: {hand_landmarks.categories.score} / categoryname: {hand_landmarks.categories.categoryName}") - - for ids, landmrk in enumerate(hand_landmarks.landmark): - cx, cy, cz = landmrk.x, landmrk.y, landmrk.z - g_hand_data[ids, 0] = cx - g_hand_data[ids, 1] = cy - g_hand_data[ids, 2] = cz - - # 정규화 - min_vals = np.min(g_hand_data, axis=0) - max_vals = np.max(g_hand_data, axis=0) - g_hand_data_normalized = (g_hand_data - min_vals) / (max_vals - min_vals) - g_hand_data_normalized[:, 2] = g_hand_data[:,2] - - mp_drawing.draw_landmarks( - image, - hand_landmarks, - mp_hands.HAND_CONNECTIONS, - # mp_drawing_styles.get_default_hand_landmarks_style(), - # mp_drawing_styles.get_default_hand_connections_style() - ) - - if results.multi_hand_world_landmarks: - for hand_world_landmarks in results.multi_hand_world_landmarks: - # Here is How to Get All the Coordinates - for ids, landmrk in enumerate(hand_world_landmarks.landmark): - # print(ids, landmrk) - cx, cy, cz = landmrk.x, landmrk.y, landmrk.z - # print(f'{ids}: {cx}, {cy}, {cz}') - g_hand_world_data[ids, 0] = cx - g_hand_world_data[ids, 1] = cy - g_hand_world_data[ids, 2] = cz - else: - # print("No Right Hand") - pass - - c_time = time.time() - fps = 1 / (c_time - p_time) - p_time = c_time - cv2.putText(image, f"FPS: {int(fps)}", (20, 20), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 1) - # cv2.putText(image, f"index finger(basic) : x:{g_hand_data[8,0]}", - # (20, 60), cv2.FONT_HERSHEY_PLAIN, 1, (255, 20, 20), 1) - # cv2.putText(image, f"index finger(basic) : y:{g_hand_data[8,1]}", - # (20, 80), cv2.FONT_HERSHEY_PLAIN, 1, (255, 20, 20), 1) - # cv2.putText(image, f"index finger(basic) : z:{g_hand_data[8,2]}", - # (20, 100), cv2.FONT_HERSHEY_PLAIN, 1, (255, 20, 20), 1) - # cv2.putText(image, f"index finger(world) : x:{g_hand_world_data[8, 0]}", - # (20, 140),cv2.FONT_HERSHEY_PLAIN, 1, (255, 20, 255), 1) - # cv2.putText(image, f"index finger(world) : y:{g_hand_world_data[8, 1]}", - # (20, 160),cv2.FONT_HERSHEY_PLAIN, 1, (255, 20, 255), 1) - # cv2.putText(image, f"index finger(world) : z:{g_hand_world_data[8, 2]}", - # (20, 180),cv2.FONT_HERSHEY_PLAIN, 1, (255, 20, 255), 1) - - pixel_x = int(g_hand_data[8, 0] * image_width) - pixel_y = int(g_hand_data[8, 1] * image_height) - # print(pixel_x, pixel_y) - if pixel_x>0 and pixel_x0 and pixel_ystd_msgs sensor_msgs geometry_msgs + custom_interfaces mediapipe rviz_visual_tools