diff --git a/src/GUI_main_program.py b/src/GUI_main_program.py index ef915b0..7a1e3b0 100644 --- a/src/GUI_main_program.py +++ b/src/GUI_main_program.py @@ -173,6 +173,11 @@ def __init__(self): help_menu.addAction(help_action) help_action.triggered.connect(self.open_help_window) + # Camera Combo Box + # Make default camera 0: Intel Realsense,1:oakDLite etc. + self.Select3DCamera.setCurrentIndex(0) + self.Select3DCamera.currentTextChanged.connect(self.on_camera_selected) + # Folder Options Hidden self.folder_list.setHidden(True) # List of folders to put the data into self.folder_name.setHidden(True) @@ -297,6 +302,10 @@ def camera_options(self): if os.path.isfile(f"{cwd}/cam_img_2.png"): os.remove(f"{cwd}/cam_img_2.png") ''' + # Function to handle camera selection + def on_camera_selected(self, Select3DCamera): + self.Select3DCamera = Select3DCamera + print(Select3DCamera) def open_help_window(self): self.help_window = HelpWindow() @@ -456,6 +465,7 @@ def define_objects_shown(self): def color_button_pressed(self): + self.tracking_info = tracking(types_of_streams_saved = 'cd', type_of_tracking = 'color', input_folder = 'color_i', @@ -965,6 +975,7 @@ def run_tracker(self): image, color_ranges, min_radius_object, data_output_folder_path = self.get_settings() # config by looking at the camera (remove this from tracking program below) # find output folder here instead of + print(self.Select3DCamera) if self.tracking_info.types_of_streams_saved =='cd': # The color and depth streams need to be aligned for each frame slowing the tracking down pipeline = find_and_config_device() @@ -975,7 +986,7 @@ def run_tracker(self): # The infrared and depth streams are already aligned # object tracking at either 90 or 300 frames per second with infrared pipeline, config = find_and_config_device_mult_stream(self.tracking_info.types_of_streams_saved) - pipeline.start(config) + GUI_obj_tracking(pipeline, image, color_ranges, min_radius_object, data_output_folder_path, self.tracking_info) diff --git a/src/tracker/lib/GUI_Base.ui b/src/tracker/lib/GUI_Base.ui index 61a973a..99c5051 100644 --- a/src/tracker/lib/GUI_Base.ui +++ b/src/tracker/lib/GUI_Base.ui @@ -511,7 +511,7 @@ Select the additional variables you would like graphed 100 - 380 + 410 371 181 @@ -594,7 +594,7 @@ Select the additional variables you would like graphed 100 - 350 + 360 101 20 @@ -610,7 +610,7 @@ Select the additional variables you would like graphed 200 - 350 + 360 131 20 @@ -623,7 +623,7 @@ Select the additional variables you would like graphed 100 - 380 + 390 431 21 @@ -929,7 +929,7 @@ Low fps 100 - 60 + 70 120 55 @@ -943,7 +943,7 @@ Low fps 230 - 60 + 70 120 55 @@ -961,7 +961,7 @@ Object tracking 100 - 140 + 150 381 22 @@ -1149,7 +1149,7 @@ Object tracking 100 - 170 + 180 381 141 @@ -1320,7 +1320,7 @@ Object tracking 360 - 60 + 70 120 55 @@ -1405,7 +1405,7 @@ Object Tracking 340 - 350 + 360 141 17 @@ -1421,7 +1421,7 @@ Object Tracking 100 - 400 + 410 431 181 @@ -1465,7 +1465,7 @@ Object Tracking - 8 + 6 @@ -1475,7 +1475,7 @@ Object Tracking - 8 + 6 @@ -1485,7 +1485,7 @@ Object Tracking - 8 + 6 @@ -1505,7 +1505,7 @@ Object Tracking - 8 + 6 @@ -1514,7 +1514,7 @@ Object Tracking 100 - 380 + 390 101 21 @@ -1533,7 +1533,7 @@ Object Tracking 380 - 380 + 390 151 23 @@ -1572,7 +1572,7 @@ Time, x,y, z (Skeletal Tracker) 620 - 60 + 70 151 55 @@ -1595,7 +1595,7 @@ and graph 490 - 60 + 70 120 55 @@ -1623,7 +1623,7 @@ and graph 100 - 40 + 50 641 21 @@ -1651,7 +1651,7 @@ and graph 100 - 310 + 320 571 41 @@ -1683,7 +1683,7 @@ Don't forget to enter the mass if you want momentum graphs. 100 - 120 + 130 571 21 @@ -1818,7 +1818,7 @@ then see it in 3D or find trendlines 100 - 400 + 410 431 181 @@ -1955,11 +1955,11 @@ Will record for a few seconds and stop Qt::PlainText - + 100 - 28 + 30 171 18 diff --git a/src/tracker/lib/GUI_real_time_color_tracker.py b/src/tracker/lib/GUI_real_time_color_tracker.py index 8424955..b4c4f9b 100644 --- a/src/tracker/lib/GUI_real_time_color_tracker.py +++ b/src/tracker/lib/GUI_real_time_color_tracker.py @@ -139,7 +139,7 @@ def __init__(self, color, callback): print('color in pipe', color) #self.current_colormap = self.HSV_COLORMAP[color] - self.lower,self.upper, self.color, self.radius_meters, self.mass = color_ranges[0] + self.lower,self.upper, self.color, self.radius_meters, self.mass = color_ranges[0] self.color = color self.color_ranges = color_ranges #self.current_colormap = self.HSV_COLORMAP[color] @@ -181,7 +181,7 @@ def run(self): if not frame_result: continue - (cv_color, rs_color, rs_depth), timestamp = frame_result + (cv_color, npy_depth, _, _), timestamp= frame_result # Start the timer if first_time_check: @@ -198,10 +198,6 @@ def run(self): i +=1 continue - # Create a colormap from the depth data TODO add if desired - #depth_image = np.asanyarray(rs_depth.get_data()) - #depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV) - #from https://github.com/IntelRealSense/librealsense/issues/2204#issuecomment-414497056 hsv = make_color_hsv(cv_color) @@ -213,7 +209,7 @@ def run(self): if x_pixel == -1: continue # get.distance is a little slower so only use if necessarycenter = round(aligned_depth_frame.get_distance(int(x),int(y)),4) - x_coord, y_coord, z_coord = get_depth_meters(x_pixel, y_pixel, self.radius_meters, rs_depth, rs_color, self.zeroed_x, self.zeroed_y, self.zeroed_z, self.z) + x_coord, y_coord, z_coord = get_depth_meters(x_pixel, y_pixel, self.radius_meters, frame_result, self.zeroed_x, self.zeroed_y, self.zeroed_z, self.z) if x_coord == -1: continue @@ -223,8 +219,7 @@ def run(self): # Create a colormap from the depth data if image.show_depth or image.save_depth: - depth_image = np.asanyarray(rs_depth.get_data()) - depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV) + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(npy_depth, alpha=0.10), cv2.COLORMAP_HSV) cv2.putText(depth_colormap, 'Time: ' + str(relative_timestamp), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50,170,50), 2) cv2.circle(depth_colormap, (int(x_pixel), int(y_pixel)), int(radius), (255, 255, 255), 2) # Show depth colormap & color feed diff --git a/src/tracker/lib/GUI_tracker.py b/src/tracker/lib/GUI_tracker.py index dbdb730..afed05c 100644 --- a/src/tracker/lib/GUI_tracker.py +++ b/src/tracker/lib/GUI_tracker.py @@ -19,7 +19,7 @@ def find_lower_upper_bounds_on_screen(the_array): # selecting your own color boundaries by tweeking the default pipeline = find_and_config_device() warm_up_camera(pipeline) - (cv_color, rs_color, rs_depth), timestamp = get_all_frames_color(pipeline) + (cv_color, npy_depth, _, _ ), timestamp = get_all_frames_color(pipeline) output = GUI_find_hsv_bounds(the_array, cv_color) return output @@ -59,7 +59,6 @@ def what_to_do_with_images(i, x_pixel, y_pixel, radius,image,depth_colormap,cv_c else: print("Error: Image not loaded correctly") - def GUI_tracking(pipeline, image, color_ranges, min_radius_object, data_output_folder_path, tracking_info): @@ -91,8 +90,9 @@ def GUI_tracking(pipeline, image, color_ranges, min_radius_object, data_output_f # Get frames if valid frame_result = get_all_frames_color(pipeline) if not frame_result: - continue - (cv_color, rs_color, rs_depth), timestamp = frame_result + continue + + (cv_color, npy_depth, _, _), timestamp = frame_result # Start the timer if first_time_check: @@ -105,8 +105,8 @@ def GUI_tracking(pipeline, image, color_ranges, min_radius_object, data_output_f ## Color Tracking by making a mask for each color tracked hsv = make_color_hsv(cv_color) - depth_image = np.asanyarray(rs_depth.get_data()) - depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(npy_depth, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data + for (lower,upper, color_name, radius_meters, mass) in color_ranges: # Find location of the object in x,y pixels using color masks @@ -128,7 +128,7 @@ def GUI_tracking(pipeline, image, color_ranges, min_radius_object, data_output_f continue # get.distance is a little slower so only use if necessarycenter = round(aligned_depth_frame.get_distance(int(x),int(y)),4) - x_coord, y_coord, z_coord = get_depth_meters(x_pixel, y_pixel, radius_meters, rs_depth, rs_color, zeroed_x, zeroed_y, zeroed_z, clipping_distance) + x_coord, y_coord, z_coord = get_depth_meters(x_pixel, y_pixel, radius_meters, frame_result, zeroed_x, zeroed_y, zeroed_z, clipping_distance) if x_coord == -1 or y_coord == -1: continue # Append to the file until there is an error at which it will close @@ -231,7 +231,7 @@ def GUI_obj_tracking(pipeline, image, color_ranges, min_radius_object, data_outp frame_result = get_all_frames_infrared(pipeline) if not frame_result: continue - (rs_depth, rs_infrared), timestamp = frame_result + (npy_infrared, npy_depth, _, _), timestamp = frame_result # Start the timer if first_time_check: @@ -242,12 +242,9 @@ def GUI_obj_tracking(pipeline, image, color_ranges, min_radius_object, data_outp # Converts time from milliseconds to seconds relative_timestamp = round(((timestamp - start_time) / 1000),3) - ## TODO use rs_infrared to see infrared from camera 1 - depth_image = np.asanyarray(rs_depth.get_data()) - infrared_image = np.array(rs_infrared.get_data()) - depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(npy_depth, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data # image the user sees as if it were color - cv_color = infrared_image + cv_color = npy_infrared ## Color Tracking by making a mask for each color tracked for (lower,upper, color_name, radius_meters, mass) in color_ranges: @@ -260,7 +257,7 @@ def GUI_obj_tracking(pipeline, image, color_ranges, min_radius_object, data_outp continue # get.distance is a little slower so only use if necessarycenter = round(aligned_depth_frame.get_distance(int(x),int(y)),4) - x_coord, y_coord, z_coord = get_depth_meters(x_pixel, y_pixel, radius_meters, rs_depth, rs_infrared, zeroed_x, zeroed_y, zeroed_z, clipping_distance) + x_coord, y_coord, z_coord = get_depth_meters(x_pixel, y_pixel, radius_meters, frame_result, zeroed_x, zeroed_y, zeroed_z, clipping_distance) if x_coord == -1: continue # Append to the file until there is an error at which it will close diff --git a/src/tracker/lib/color.py b/src/tracker/lib/color.py index 2f82398..271efef 100644 --- a/src/tracker/lib/color.py +++ b/src/tracker/lib/color.py @@ -34,7 +34,7 @@ def GUI_find_hsv_bounds(the_array, src) -> Optional[np.ndarray]: cap.set(4,480) else: # TODO If the webcam doesn't work have another option to change camera - frame = src #The rs_color frame as a numpy array + frame = src # Create a window named Press (s) when done. cv2.namedWindow("Press (s) when done") @@ -65,7 +65,7 @@ def GUI_find_hsv_bounds(the_array, src) -> Optional[np.ndarray]: else: try: if isinstance(frame, np.ndarray): - frame = src #The rs_color frame as a numpy array + frame = src except NameError: break @@ -312,16 +312,19 @@ def find_object_by_color_with_red(cv_color, color, color_ranges) -> Tuple[Option else: return None, None, None, None -def find_Time_x_y_z_from_rs_frames(i,save_image, show_image, cv_color, rs_color, rs_depth, relative_timestamp, data_output_folder_path, color_ranges, zeroed_x, zeroed_y, zeroed_z, clipping_distance, min_radius_of_object, max_num_point): +def find_Time_x_y_z_from_rs_frames(i,save_image, show_image, frame_result, relative_timestamp, data_output_folder_path, color_ranges, zeroed_x, zeroed_y, zeroed_z, clipping_distance, min_radius_of_object, max_num_point): ###TODO setup the trailing image so this will work for other data as well. + ## NOT USED righ now + + (cv_color, npy_depth, _, _), timestamp= frame_result + save_trailing_image = False if save_trailing_image: trailing_color = cv2.imread(data_output_folder_path + '/'+ 'trailing'+'.jpg') ## TODO change this to save depth image and show depth image if save_image: - depth_image = np.asanyarray(rs_depth.get_data()) - depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV) + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(npy_depth, alpha=0.10), cv2.COLORMAP_HSV) hsv = make_color_hsv(cv_color) x_pixel = None @@ -336,7 +339,7 @@ def find_Time_x_y_z_from_rs_frames(i,save_image, show_image, cv_color, rs_color, # Get the distance using the points around the center of the object depth_near_center = [] for pixel_count in range(-1,2): - _, _, z_coord = get_depth_meters(x_pixel+pixel_count, y_pixel+pixel_count, radius_meters, rs_depth, rs_color, zeroed_x, zeroed_y, zeroed_z, clipping_distance) + _, _, z_coord = get_depth_meters(x_pixel+pixel_count, y_pixel+pixel_count, radius_meters, frame_result, zeroed_x, zeroed_y, zeroed_z, clipping_distance) if z_coord is not None: depth_near_center.append(z_coord) # Median for our 5 points @@ -346,7 +349,7 @@ def find_Time_x_y_z_from_rs_frames(i,save_image, show_image, cv_color, rs_color, continue # Get the x and y_coord at the exact center of the object - x_coord, y_coord, _ = get_depth_meters(x_pixel, y_pixel, radius_meters, rs_depth, rs_color, zeroed_x, zeroed_y, zeroed_z, clipping_distance) + x_coord, y_coord, _ = get_depth_meters(x_pixel, y_pixel, radius_meters, frame_result, zeroed_x, zeroed_y, zeroed_z, clipping_distance) z_coord = depth_near_center[floor((len(depth_near_center)-1)/2)] diff --git a/src/tracker/lib/intel_realsense_D435i.py b/src/tracker/lib/intel_realsense_D435i.py index 188816f..cea161f 100644 --- a/src/tracker/lib/intel_realsense_D435i.py +++ b/src/tracker/lib/intel_realsense_D435i.py @@ -31,6 +31,8 @@ def find_and_config_device(): config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 60) config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 60) profile = pipeline.start(config) + depth_sensor = profile.get_device().first_depth_sensor() + depth_scale = depth_sensor.get_depth_scale() warm_up_camera(pipeline) # Let the camera run for a few seconds so you do not get dark images @@ -51,7 +53,11 @@ def find_and_config_device_mult_stream(types_of_streams_saved) -> Any: config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 60) if types_of_streams_saved == 'all': config.enable_stream(rs.stream.infrared, 1, 848, 480, rs.format.y8, 60) - + + profile = pipeline.start(config) + depth_sensor = profile.get_device().first_depth_sensor() + depth_scale = depth_sensor.get_depth_scale() + print("Depth Scale is: ", depth_scale) # If you want to use multiple cameras in the future this is useful #ctx = rs.context() #if len(ctx.devices) > 0: @@ -89,9 +95,13 @@ def record_bag_file(data_output_folder_path, types_of_streams_saved): # For now, just use the Intel Real Sense View if you installed it # https://www.intelrealsense.com/sdk-2/ filepath_bag = os.path.abspath(os.path.join(data_output_folder_path, 'bag.bag')) - pipeline, config = find_and_config_device_mult_stream(types_of_streams_saved) + pipeline,config = find_and_config_device_mult_stream(types_of_streams_saved) config.enable_record_to_file(filepath_bag) pipeline.start(config) + + # Get depth scale for later use + profile = pipeline.get_active_profile() + warm_up_camera(pipeline) time.sleep(1) #wait_to_start = input('Hit enter to start and stop recording') @@ -101,6 +111,7 @@ def record_bag_file(data_output_folder_path, types_of_streams_saved): if types_of_streams_saved == 'cd': frames_to_record = time_to_record * 60 elif types_of_streams_saved == 'id300': frames_to_record = time_to_record * 300 elif types_of_streams_saved == 'id': frames_to_record = time_to_record * 90 + else: frames_to_record = time_to_record * 60 # TODO Change this to while true and break at space bar when done testing for _ in range(frames_to_record): @@ -113,7 +124,7 @@ def record_bag_file(data_output_folder_path, types_of_streams_saved): # read_bag_file_and_config(types_of_streams_saved, data_output_folder_path, 'bag', filepath_bag) print('done recording') -def get_all_frames_color(rs_pipeline) -> Optional[Tuple[Tuple[Any, Any, Any], Any]]: +def get_all_frames_color(rs_pipeline) -> Optional[Tuple[Tuple[np.ndarray, np.ndarray, Any, Any], Any]]: ''' Returns a tuple containing the OpenCV color and (aligned) RealSense depth/color frames, as well as the timestamp of the frames. If either frame @@ -130,17 +141,19 @@ def get_all_frames_color(rs_pipeline) -> Optional[Tuple[Tuple[Any, Any, Any], An # Extract color/depth frames rs_color = rs_frames_aligned.get_color_frame() - rs_depth = rs_frames_aligned.get_depth_frame().as_depth_frame() - cv_color = np.array(rs_color.get_data()) - + rs_depth = rs_frames_aligned.get_depth_frame() # Check that both frames are valid & return false if they aren't if not rs_depth or not rs_color: return None + + cv_color = np.array(rs_color.get_data()) + rs_depth = rs_frames_aligned.get_depth_frame().as_depth_frame() + npy_depth = np.asanyarray(rs_depth.get_data()) # Calculate elapsed time from start_datetime, if applicable - return (cv_color, rs_color, rs_depth), timestamp + return (cv_color, npy_depth, rs_color, rs_depth), timestamp -def get_all_frames_infrared(rs_pipeline) -> Optional[Tuple[Tuple[Any, Any, Any], Any]]: +def get_all_frames_infrared(rs_pipeline) -> Optional[Tuple[Tuple[np.ndarray, np.ndarray, Any, Any], Any]]: ''' Returns a tuple containing the OpenCV color and (aligned) RealSense depth/color frames, as well as the timestamp of the frames. If either frame @@ -151,16 +164,19 @@ def get_all_frames_infrared(rs_pipeline) -> Optional[Tuple[Tuple[Any, Any, Any], timestamp = rs_frames.get_timestamp() # Extract color/depth frames - rs_infrared = rs_frames.get_infrared_frame() + rs_infrared1 = rs_frames.get_infrared_frame() + npy_infrared = np.asanyarray(rs_infrared1.get_data()) rs_depth = rs_frames.get_depth_frame().as_depth_frame() + npy_depth = np.asanyarray(rs_depth.get_data()) + #npy_depth = np.asanyarray(rs_depth.get_data()).astype(np.float64) + #npy_depth_meters = npy_depth * depth_scale # Check that both frames are valid & return false if they aren't if not rs_depth: return None # Calculate elapsed time from start_datetime, if applicable - return (rs_depth, rs_infrared), timestamp - + return (npy_infrared, npy_depth, rs_infrared1, rs_depth),timestamp def select_clipping_distance(frame, rs_depth) -> Tuple[Any, float, float]: # This selects both the background and the TODO Origin @@ -174,9 +190,10 @@ def select_clipping_distance(frame, rs_depth) -> Tuple[Any, float, float]: clipping_distance = depth * 0.99 return wall, depth, clipping_distance -def get_coordinates_meters(rs_frame, rs_depth, x_pixel: int, y_pixel: int, depth) -> Tuple[float, float, float]: +def get_coordinates_meters(frame_result, x_pixel: int, y_pixel: int, depth) -> Tuple[float, float, float]: # Given the x,y in pixels find the x,y,z in meters + (cv_color, npy_depth, rs_frame, rs_depth), timestamp= frame_result depth_pixel = [int(x_pixel),int(y_pixel)] # object pixel depth_intrin = rs_depth.profile.as_video_stream_profile().intrinsics depth_to_frame_extrin = rs_depth.profile.get_extrinsics_to(rs_frame.profile) @@ -186,16 +203,21 @@ def get_coordinates_meters(rs_frame, rs_depth, x_pixel: int, y_pixel: int, depth frame_point = rs.rs2_transform_point_to_point(depth_to_frame_extrin, depth_point) return frame_point -def select_location(frame, rs_frame, rs_depth) -> Tuple[float, Tuple[float, float, float], float]: +def select_location(frame_result) -> Tuple[float, Tuple[float, float, float], float]: # This selects both the background and the TODO Origin # Find distance (depth) to wall & store as clipping distance # Note that it is reduced slightly to allow some room for error (*0.95) + + (frame, npy_depth, _, rs_depth), timestamp= frame_result wall, depth, clipping_distance = select_clipping_distance(frame, rs_depth) depth = round(rs_depth.get_distance(int(wall[0]),int(wall[1])),4) - frame_point = get_coordinates_meters(rs_frame, rs_depth, wall[0], wall[1], depth) + frame_point = get_coordinates_meters(frame_result, wall[0], wall[1], depth) return clipping_distance, frame_point, depth -def get_depth_meters(x_pixel, y_pixel, radius_meters, rs_depth, rs_frame, zeroed_x, zeroed_y, zeroed_z, clipping_distance) -> Tuple[Optional[float], Optional[float], Optional[float]]: +def get_depth_meters(x_pixel, y_pixel, radius_meters, frame_result, zeroed_x, zeroed_y, zeroed_z, clipping_distance) -> Tuple[Optional[float], Optional[float], Optional[float]]: + + (_, npy_depth, _, rs_depth), timestamp= frame_result + try: depth = round(rs_depth.get_distance(int(x_pixel),int(y_pixel)),4) if (depth - radius_meters) < 0.1: @@ -209,7 +231,7 @@ def get_depth_meters(x_pixel, y_pixel, radius_meters, rs_depth, rs_frame, zeroed return -1, -1, -1 # Given the location in pixels for x,y and the depth, find the coordinates in meters - frame_point = get_coordinates_meters(rs_frame, rs_depth, x_pixel, y_pixel, depth) + frame_point = get_coordinates_meters(frame_result, x_pixel, y_pixel, depth) # Round all 3 coordinates to 5 decimal places x_coord = round(frame_point[0], 5) @@ -251,9 +273,8 @@ def select_furthest_distance_color(pipeline) -> Tuple[float, float, float, float frame_result = get_all_frames_color(pipeline) if not frame_result: continue - (cv_color, rs_color, rs_depth), _ = frame_result - # Find the clipping distance - clipping_distance, color_point, zeroed_z = select_location(cv_color, rs_color, rs_depth) + # Find the clipping distance + clipping_distance, color_point, zeroed_z = select_location(frame_result) print('clipping distance is', clipping_distance) #TODO later call above to find the origin then call the following after # for now this just sets all zeroed values as 0 insted of a specific origin, so the origin is the cent of the camera @@ -283,14 +304,12 @@ def select_furthest_distance_infrared(pipeline): frame_result = get_all_frames_infrared(pipeline) if not frame_result: continue - (rs_depth, rs_infrared), _ = frame_result - if not rs_infrared: - continue - infrared_image = np.asanyarray(rs_infrared.get_data()) + # Find distance (depth) to wall & store as clipping distance # Note that it is reduced slightly to allow some room for error (*0.95) # Find the clipping distance - clipping_distance, depth_point, zeroed_z = select_location(infrared_image, rs_infrared, rs_depth) + clipping_distance, depth_point, zeroed_z = select_location(frame_result) + print('cliping distance is', clipping_distance) #TODO later call above to find the origin then call the following after # for now this just sets all zeroed values as 0 instead of a specific origin, so the origin is the cent of the camera zeroed_x, zeroed_y, zeroed_z = set_the_origin (depth_point) diff --git a/src/tracker/lib/object_tracking.py b/src/tracker/lib/object_tracking.py index 05c04b0..a1055c8 100644 --- a/src/tracker/lib/object_tracking.py +++ b/src/tracker/lib/object_tracking.py @@ -13,12 +13,10 @@ def GUI_select_bounding_box(pipeline): frame_result = get_all_frames_color(pipeline) if not frame_result: continue - (cv_color, rs_color, rs_depth), _ = frame_result + (color_image, npy_depth,_,_), _ = frame_result - ## TODO use rs_infrared to see infrared from camera 1 - depth_image = np.asanyarray(rs_depth.get_data()) - color_image = np.asanyarray(rs_color.get_data()) - depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data + + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(npy_depth, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data # User inputs the type of tracking used tracker = select_object_tracker_method() # Select the object to track @@ -26,7 +24,7 @@ def GUI_select_bounding_box(pipeline): bbox = cv2.selectROI('ROI Selection', color_image, False) ret = tracker.init(depth_colormap, bbox) print(bbox) - cv2.destroyWindow('ROI Selection') + cv2.destroyWindow('ROI Selection') check_no_selection = False return bbox, ret, tracker @@ -37,17 +35,14 @@ def GUI_select_bounding_box_infrared(pipeline): frame_result = get_all_frames_infrared(pipeline) if not frame_result: continue - (rs_depth, rs_infrared1), _ = frame_result - - ## TODO use rs_infrared to see infrared from camera 1 - depth_image = np.asanyarray(rs_depth.get_data()) - infrared_image = np.asanyarray(rs_infrared1.get_data()) - depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data + (npy_infrared1, npy_depth, _, _), _ = frame_result + + depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(npy_depth, alpha=0.10), cv2.COLORMAP_HSV)# Create a colormap from the depth data # User inputs the type of tracking used tracker = select_object_tracker_method() # Select the object to track print('select object') - bbox = cv2.selectROI('ROI Selection', infrared_image, False) + bbox = cv2.selectROI('ROI Selection', npy_infrared1, False) ret = tracker.init(depth_colormap, bbox) print(bbox) cv2.destroyWindow('ROI Selection')