def ingest_in_objects_queue(self, batch): ''' ingest_in_objects_queue: Converts a list of batched objects from SDK get_objects_batch() to a sorted list of sl.Objects Use this function to fill a deque of sl.Objects that can be considered and used as a stream of objects with a delay Parameters: batch (list[sl.ObjectsBatch]): list of sl.ObjectsBatch objects obtained from calling get_objects_batch ''' # If the list is empty, do nothing if not batch: return # Add objects in dict with timestamp as key list_of_new_objects = {} for current_traj in batch: # Impossible but still better to check if len(current_traj.timestamps) != len(current_traj.positions): continue # For each sample, construct an ObjectData and put it in the corresponding sl.Objects for i in range(len(current_traj.timestamps)): ts = current_traj.timestamps[i] new_object_data = sl.ObjectData() new_object_data.id = current_traj.id new_object_data.tracking_state = current_traj.tracking_state new_object_data.position = current_traj.positions[i] new_object_data.label = current_traj.label new_object_data.sublabel = current_traj.sublabel new_object_data.bounding_box_2d = current_traj.bounding_boxes_2d[ i] new_object_data.bounding_box = current_traj.bounding_boxes[i] # Check if a detected object with the current timestamp already exists in the map if (ts.get_milliseconds() in list_of_new_objects.keys()): # Append new_object_data to the object_list list_of_new_objects[ts.get_milliseconds( )].object_list = list_of_new_objects[ ts.get_milliseconds()].object_list + [new_object_data] else: current_obj = sl.Objects() current_obj.timestamp = ts.data_ns # sl.Objects.timestamp can be set with time in nanoseconds current_obj.is_new = True current_obj.is_tracked = True # Append new_object_data to the object_list current_obj.object_list = current_obj.object_list + [ new_object_data ] list_of_new_objects[ts.get_milliseconds()] = current_obj # Ingest in Queue of objects that will be emptied by the main loop # Since dicts are sorted by key, we are sure that timestamps are continous. for value in list_of_new_objects.values(): self.objects_tracked_queue.append(value)
def main(): # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode init_params.camera_fps = 15 # Set fps at 15 # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Capture 50 frames and stop mat = sl.Mat() runtime_parameters = sl.RuntimeParameters() key = '' obj_param = sl.ObjectDetectionParameters() obj_param.enable_tracking = False zed.enable_object_detection(obj_param) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 40 while key != 113: # for 'q' key # Grab an image, a RuntimeParameters object must be given to grab() if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # A new image is available if grab() returns SUCCESS zed.retrieve_image(mat, sl.VIEW.LEFT) zed.retrieve_objects(objects, obj_runtime_param) obj_array = objects.object_list image_data = mat.get_data() for i in range(len(obj_array)): obj_data = obj_array[i] bounding_box = obj_data.bounding_box_2d cv2.rectangle( image_data, (int(bounding_box[0, 0]), int(bounding_box[0, 1])), (int(bounding_box[2, 0]), int(bounding_box[2, 1])), get_color_id_gr(int(obj_data.id)), 3) cv2.imshow("ZED", image_data) key = cv2.waitKey(5) cv2.destroyAllWindows() # Close the camera zed.close()
def pop(self, local_pose, world_pose, image, depth, objects): ''' pop pop data from the FIFO system Parameters: local_pose (sl.Pose): pose of the camera in camera reference frame at objects timestamp world_pose (sl.Pose): pose of the camera in world reference frame at objects timestamp image (sl.Mat): image data at objects timestamp depth (sl.Mat): depth data at objects timestamp objects (sl.Objects): objects in the past ''' objects = sl.Objects() local_pose = sl.Pose() world_pose = sl.Pose() if self.objects_tracked_queue: tracked_merged_obj = self.objects_tracked_queue[0] if (self.init_queue_ts.data_ns == 0): self.init_queue_ts = tracked_merged_obj.timestamp targetTS_ms = tracked_merged_obj.timestamp.get_milliseconds() local_pose = self.find_closest_local_pose_from_ts(targetTS_ms) world_pose = self.find_closest_world_pose_from_ts(targetTS_ms) if WITH_IMAGE_RETENTION: tmp_image = self.find_closest_image_from_ts(targetTS_ms) tmp_image.copy_to(image) tmp_image.free(sl.MEM.CPU) self.image_map_ms[targetTS_ms].free(sl.MEM.CPU) del self.image_map_ms[targetTS_ms] tmp_depth = self.find_closest_depth_from_ts(targetTS_ms) tmp_depth.copy_to(depth) tmp_depth.free(sl.MEM.CPU) self.depth_map_ms[targetTS_ms].free(sl.MEM.CPU) del self.depth_map_ms[targetTS_ms] objects = tracked_merged_obj self.objects_tracked_queue.popleft() return local_pose, world_pose, image, depth, objects
def __init__(self): # 初始化关于摄像头的参数 self.zed2 = sl.Camera() self.init_params = sl.InitParameters() self.init_params.camera_resolution = sl.RESOLUTION.VGA self.init_params.camera_fps = 15 # default 15 self.init_params.coordinate_units = sl.UNIT.METER self.init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE _ = self.zed2.open(self.init_params) self.mat = sl.Mat() self.objects = sl.Objects() self.point_cloud = sl.Mat() self.runtime_params = sl.RuntimeParameters() self.runtime_params.sensing_mode = sl.SENSING_MODE.STANDARD self.obj_param = sl.ObjectDetectionParameters() self.obj_param.enable_tracking = False self.zed2.enable_object_detection(self.obj_param) self.obj_runtime_params = sl.ObjectDetectionRuntimeParameters() self.obj_runtime_params.detection_confidence_threshold = 40
zed.enable_positional_tracking() zed.enable_object_detection(obj_param) camera_info = zed.get_camera_information() # Create OpenGL viewer viewer = gl.GLViewer() viewer.init(camera_info.calibration_parameters.left_cam) # Configure object detection runtime parameters obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 50 obj_runtime_param.object_class_filter = [sl.OBJECT_CLASS.PERSON] # Only detect Persons # Create ZED objects filled in the main loop objects = sl.Objects() image = sl.Mat() while viewer.is_available(): # Grab an image, a RuntimeParameters object must be given to grab() if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Retrieve left image zed.retrieve_image(image, sl.VIEW.LEFT) # Retrieve objects zed.retrieve_objects(objects, obj_runtime_param) # Update GL view viewer.update_view(image, objects) viewer.exit() image.free(memory_type=sl.MEM.CPU)
def main(): # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE init_params.coordinate_units = sl.UNIT.METER init_params.sdk_verbose = True # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) obj_param = sl.ObjectDetectionParameters() # Different model can be chosen, optimizing the runtime or the accuracy obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_FAST obj_param.enable_tracking = True obj_param.image_sync = True obj_param.enable_mask_output = False # Optimize the person joints position, requires more computations obj_param.enable_body_fitting = True camera_infos = zed.get_camera_information() if obj_param.enable_tracking: positional_tracking_param = sl.PositionalTrackingParameters() # positional_tracking_param.set_as_static = True positional_tracking_param.set_floor_as_origin = True zed.enable_positional_tracking(positional_tracking_param) print("Object Detection: Loading Module...") err = zed.enable_object_detection(obj_param) if err != sl.ERROR_CODE.SUCCESS: print(repr(err)) zed.close() exit(1) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() # For outdoor scene or long range, the confidence should be lowered to avoid missing detections (~20-30) # For indoor scene or closer range, a higher confidence limits the risk of false positives and increase the precision (~50+) obj_runtime_param.detection_confidence_threshold = 40 while zed.grab() == sl.ERROR_CODE.SUCCESS: err = zed.retrieve_objects(objects, obj_runtime_param) if objects.is_new: obj_array = objects.object_list print(str(len(obj_array)) + " Person(s) detected\n") if len(obj_array) > 0: first_object = obj_array[0] print("First Person attributes:") print(" Confidence (" + str(int(first_object.confidence)) + "/100)") if obj_param.enable_tracking: print(" Tracking ID: " + str(int(first_object.id)) + " tracking state: " + repr( first_object.tracking_state) + " / " + repr(first_object.action_state)) position = first_object.position velocity = first_object.velocity dimensions = first_object.dimensions print(" 3D position: [{0},{1},{2}]\n Velocity: [{3},{4},{5}]\n 3D dimentions: [{6},{7},{8}]".format( position[0], position[1], position[2], velocity[0], velocity[1], velocity[2], dimensions[0], dimensions[1], dimensions[2])) if first_object.mask.is_init(): print(" 2D mask available") print(" Keypoint 2D ") keypoint_2d = first_object.keypoint_2d for it in keypoint_2d: print(" " + str(it)) print("\n Keypoint 3D ") keypoint = first_object.keypoint for it in keypoint: print(" " + str(it)) input('\nPress enter to continue: ') # Close the camera zed.close()
def main(): # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode #init_params.camera_fps = 15 # Set fps at 15 # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Capture 50 frames and stop mat = sl.Mat() runtime_parameters = sl.RuntimeParameters() key = '' zed.enable_positional_tracking() obj_param = sl.ObjectDetectionParameters() obj_param.enable_tracking = True obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_FAST zed.enable_object_detection(obj_param) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 50 while key != 113: # for 'q' key # Grab an image, a RuntimeParameters object must be given to grab() if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # A new image is available if grab() returns SUCCESS zed.retrieve_image(mat, sl.VIEW.LEFT) zed.retrieve_objects(objects, obj_runtime_param) obj_array = objects.object_list image_data = mat.get_data() for i in range(len(obj_array)): obj_data = obj_array[i] bounding_box = obj_data.bounding_box_2d cv2.rectangle( image_data, (int(bounding_box[0, 0]), int(bounding_box[0, 1])), (int(bounding_box[2, 0]), int(bounding_box[2, 1])), get_color_id_gr(int(obj_data.id)), 3) keypoint = obj_data.keypoint_2d for kp in keypoint: if kp[0] > 0 and kp[1] > 0: cv2.circle(image_data, (int(kp[0]), int(kp[1])), 3, get_color_id_gr(int(obj_data.id)), -1) for bone in sl.BODY_BONES: kp1 = keypoint[bone[0].value] kp2 = keypoint[bone[1].value] if kp1[0] > 0 and kp1[1] > 0 and kp2[0] > 0 and kp2[1] > 0: cv2.line(image_data, (int(kp1[0]), int(kp1[1])), (int(kp2[0]), int(kp2[1])), get_color_id_gr(int(obj_data.id)), 2) cv2.imshow("ZED", image_data) key = cv2.waitKey(5) cv2.destroyAllWindows() # Close the camera zed.close()
def main(): # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE init_params.coordinate_units = sl.UNIT.METER init_params.sdk_verbose = True # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) obj_param = sl.ObjectDetectionParameters() obj_param.enable_tracking=True obj_param.image_sync=True obj_param.enable_mask_output=True camera_infos = zed.get_camera_information() if obj_param.enable_tracking : positional_tracking_param = sl.PositionalTrackingParameters() #positional_tracking_param.set_as_static = True positional_tracking_param.set_floor_as_origin = True zed.enable_positional_tracking(positional_tracking_param) print("Object Detection: Loading Module...") err = zed.enable_object_detection(obj_param) if err != sl.ERROR_CODE.SUCCESS : print (repr(err)) zed.close() exit(1) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 40 while zed.grab() == sl.ERROR_CODE.SUCCESS: err = zed.retrieve_objects(objects, obj_runtime_param) if objects.is_new : obj_array = objects.object_list print(str(len(obj_array))+" Object(s) detected\n") if len(obj_array) > 0 : first_object = obj_array[0] print("First object attributes:") print(" Label '"+repr(first_object.label)+"' (conf. "+str(int(first_object.confidence))+"/100)") if obj_param.enable_tracking : print(" Tracking ID: "+str(int(first_object.id))+" tracking state: "+repr(first_object.tracking_state)+" / "+repr(first_object.action_state)) position = first_object.position velocity = first_object.velocity dimensions = first_object.dimensions print(" 3D position: [{0},{1},{2}]\n Velocity: [{3},{4},{5}]\n 3D dimentions: [{6},{7},{8}]".format(position[0],position[1],position[2],velocity[0],velocity[1],velocity[2],dimensions[0],dimensions[1],dimensions[2])) if first_object.mask.is_init(): print(" 2D mask available") print(" Bounding Box 2D ") bounding_box_2d = first_object.bounding_box_2d for it in bounding_box_2d : print(" "+str(it),end='') print("\n Bounding Box 3D ") bounding_box = first_object.bounding_box for it in bounding_box : print(" "+str(it),end='') input('\nPress enter to continue: ') # Close the camera zed.close()
# 2D viewer utilities display_resolution = sl.Resolution( min(camera_info.camera_resolution.width, 1280), min(camera_info.camera_resolution.height, 720)) image_scale = [ display_resolution.width / camera_info.camera_resolution.width, display_resolution.height / camera_info.camera_resolution.height ] # Create OpenGL viewer viewer = gl.GLViewer() viewer.init(camera_info.calibration_parameters.left_cam, obj_param.enable_tracking) # Create ZED objects filled in the main loop bodies = sl.Objects() image = sl.Mat() while viewer.is_available(): # Grab an image if zed.grab() == sl.ERROR_CODE.SUCCESS: # Retrieve left image zed.retrieve_image(image, sl.VIEW.LEFT, sl.MEM.CPU, display_resolution) # Retrieve objects zed.retrieve_objects(bodies, obj_runtime_param) # Update GL view viewer.update_view(image, bodies) # Update OCV view image_left_ocv = image.get_data()
def main(): # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE init_params.coordinate_units = sl.UNIT.METER init_params.sdk_verbose = True # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) obj_param = sl.ObjectDetectionParameters() obj_param.enable_tracking=True obj_param.image_sync=True obj_param.enable_mask_output=True camera_infos = zed.get_camera_information() if obj_param.enable_tracking : positional_tracking_param = sl.PositionalTrackingParameters() #positional_tracking_param.set_as_static = True positional_tracking_param.set_floor_as_origin = True zed.enable_positional_tracking(positional_tracking_param) print("Object Detection: Loading Module...") err = zed.enable_object_detection(obj_param) if err != sl.ERROR_CODE.SUCCESS : print (repr(err)) zed.close() exit(1) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 40 while zed.grab() == sl.ERROR_CODE.SUCCESS: err = zed.retrieve_objects(objects, obj_runtime_param) start = timeit.default_timer() if objects.is_new : obj_array = objects.object_list if len(obj_array) > 0 : first_object = obj_array[0] print("First object attributes:") print(" Label '"+repr(first_object.label)+"' (conf. "+str(int(first_object.confidence))+"/100)") position = first_object.position dimensions = first_object.dimensions print(" 3D position: [{0},{1},{2}]\n 3D dimentions: [{3},{4},{5}]".format(position[0],position[1],position[2],dimensions[0],dimensions[1],dimensions[2])) ###################### image = sl.Mat() depth = sl.Mat() point_cloud = sl.Mat() mirror_ref = sl.Transform() mirror_ref.set_translation(sl.Translation(2.75,4.0,0)) tr_np = mirror_ref.m zed.retrieve_image(image, sl.VIEW.LEFT) # Retrieve depth map. Depth is aligned on the left image zed.retrieve_measure(depth, sl.MEASURE.DEPTH) # Retrieve colored point cloud. Point cloud is aligned on the left image. zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) x = round(image.get_width() / 2) y = round(image.get_height() / 2) err, point_cloud_value = point_cloud.get_value(x, y) distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[1] * point_cloud_value[1] + point_cloud_value[2] * point_cloud_value[2]) point_cloud_np = point_cloud.get_data() point_cloud_np.dot(tr_np) if not np.isnan(distance) and not np.isinf(distance): print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(x, y, distance), end="\r") else: pass print("\n Bounding Box 3D ") bounding_box = first_object.bounding_box stop = timeit.default_timer() print("\n FPS:", stop - start) # Close the camera zed.close()