Ejemplo n.º 1
0
    def _prep_configuration(
        self,
        color_frame_size=None,
        color_fps=None,
        depth_frame_size=None,
        depth_fps=None,
    ):
        config = rs.config()

        # only use these two formats
        color_format = rs.format.yuyv
        depth_format = rs.format.z16

        config.enable_stream(
            rs.stream.depth,
            depth_frame_size[0],
            depth_frame_size[1],
            depth_format,
            depth_fps,
        )

        config.enable_stream(
            rs.stream.color,
            color_frame_size[0],
            color_frame_size[1],
            color_format,
            color_fps,
        )

        return config
Ejemplo n.º 2
0
 def is_streaming(device_id):
     try:
         c = rs.config()
         c.enable_device(device_id)  # device_id is in fact the serial_number
         p = rs.pipeline()
         p.start(c)
         p.stop()
         return False
     except RuntimeError:
         return True
Ejemplo n.º 3
0
 def _get_default_config(self):
     config = rs.config()  # default config is RGB8, we want YUYV
     config.enable_stream(
         rs.stream.color,
         DEFAULT_COLOR_SIZE[0],
         DEFAULT_COLOR_SIZE[1],
         rs.format.yuyv,
         DEFAULT_COLOR_FPS,
     )
     config.enable_stream(
         rs.stream.depth,
         DEFAULT_DEPTH_SIZE[0],
         DEFAULT_DEPTH_SIZE[1],
         rs.format.z16,
         DEFAULT_DEPTH_FPS,
     )
     return config
Ejemplo n.º 4
0
def pyreassenseCap():

    # Configure depth and color streams
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

    # Start streaming
    pipeline.start(config)

    try:
        while True:

            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()
            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            if not depth_frame or not color_frame:
                continue

            # Convert images to numpy arrays
            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())

            # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

            # Stack both images horizontally
            images = np.hstack((color_image, depth_colormap))

            # Show images
            cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
            cv2.imshow('RealSense', images)
            cv2.imshow('depth_image',depth_image)

            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:

        # Stop streaming
        pipeline.stop()
    def __init__(self, context, pipeline_configuration):
        """
        Class to manage the Intel RealSense devices

        Parameters:
        -----------
        context 	: rs.context()
                                     The context created for using the realsense library
        pipeline_configuration 	: rs.config()
                                   The realsense library configuration to be used for the application

        """
        assert isinstance(context, type(rs.context()))
        assert isinstance(pipeline_configuration, type(rs.config()))
        self._context = context
        self._available_devices = enumerate_connected_devices(context)
        self._enabled_devices = {}
        self._config = pipeline_configuration
        self._frame_counter = 0
    def enable_imu_device(self, serial_no):
        self.pipeline = rs.pipeline()
        cfg = rs.config()
        cfg.enable_device(serial_no)
        try:
            self.pipeline.start(cfg)
        except Exception as e:
            print('ERROR: ', str(e))
            return False

        # self.sync_imu_by_this_stream = rs.stream.any
        active_imu_profiles = []

        active_profiles = dict()
        self.imu_sensor = None
        for sensor in self.pipeline.get_active_profile().get_device().sensors:
            for pr in sensor.get_stream_profiles():
                if pr.stream_type() == rs.stream.gyro and pr.format() == rs.format.motion_xyz32f:
                    active_profiles[pr.stream_type()] = pr
                    self.imu_sensor = sensor
                if pr.stream_type() == rs.stream.accel and pr.format() == rs.format.motion_xyz32f:
                    active_profiles[pr.stream_type()] = pr
                    self.imu_sensor = sensor
            if self.imu_sensor:
                break
        if not self.imu_sensor:
            print('No IMU sensor found.')
            return False
        print('\n'.join(['FOUND %s with fps=%s' % (str(ap[0]).split('.')[1].upper(), ap[1].fps()) for ap in active_profiles.items()]))
        active_imu_profiles = list(active_profiles.values())
        if len(active_imu_profiles) < 2:
            print('Not all IMU streams found.')
            return False
        self.imu_sensor.stop()
        self.imu_sensor.close()
        self.imu_sensor.open(active_imu_profiles)
        self.imu_start_loop_time = time.time()
        self.imu_sensor.start(self.imu_callback)

        # Make the device use the original IMU values and not already calibrated:
        if self.imu_sensor.supports(rs.option.enable_motion_correction):
            self.imu_sensor.set_option(rs.option.enable_motion_correction, 0)
        return True
Ejemplo n.º 7
0
def run_demo():

    # Define some constants
    resolution_width = 1280  # pixels
    resolution_height = 720  # pixels
    frame_rate = 15  # fps
    dispose_frames_for_stablisation = 30  # frames

    chessboard_width = 6  # squares
    chessboard_height = 9  # squares
    square_size = 0.0253  # meters

    try:
        # Enable the streams from all the intel realsense devices
        rs_config = rs.config()
        rs_config.enable_stream(rs.stream.depth, resolution_width,
                                resolution_height, rs.format.z16, frame_rate)
        rs_config.enable_stream(rs.stream.infrared, 1, resolution_width,
                                resolution_height, rs.format.y8, frame_rate)
        rs_config.enable_stream(rs.stream.color, resolution_width,
                                resolution_height, rs.format.bgr8, frame_rate)

        # Use the device manager class to enable the devices and get the frames
        device_manager = DeviceManager(rs.context(), rs_config)
        device_manager.enable_all_devices()

        # Allow some frames for the auto-exposure controller to stablise
        for frame in range(dispose_frames_for_stablisation):
            frames = device_manager.poll_frames()

        assert (len(device_manager._available_devices) > 0)
        """
		1: Calibration
		Calibrate all the available devices to the world co-ordinates.
		For this purpose, a chessboard printout for use with opencv based calibration process is needed.
		
		"""
        # Get the intrinsics of the realsense device
        intrinsics_devices = device_manager.get_device_intrinsics(frames)

        # Set the chessboard parameters for calibration
        chessboard_params = [chessboard_height, chessboard_width, square_size]

        # Estimate the pose of the chessboard in the world coordinate using the Kabsch Method
        calibrated_device_count = 0
        while calibrated_device_count < len(device_manager._available_devices):
            frames = device_manager.poll_frames()
            pose_estimator = PoseEstimation(frames, intrinsics_devices,
                                            chessboard_params)
            transformation_result_kabsch = pose_estimator.perform_pose_estimation(
            )
            object_point = pose_estimator.get_chessboard_corners_in3d()
            calibrated_device_count = 0
            for device in device_manager._available_devices:
                if not transformation_result_kabsch[device][0]:
                    print(
                        "Place the chessboard on the plane where the object needs to be detected.."
                    )
                else:
                    calibrated_device_count += 1

        # Save the transformation object for all devices in an array to use for measurements
        transformation_devices = {}
        chessboard_points_cumulative_3d = np.array([-1, -1, -1]).transpose()
        for device in device_manager._available_devices:
            transformation_devices[device] = transformation_result_kabsch[
                device][1].inverse()
            points3D = object_point[device][2][:, object_point[device][3]]
            points3D = transformation_devices[device].apply_transformation(
                points3D)
            chessboard_points_cumulative_3d = np.column_stack(
                (chessboard_points_cumulative_3d, points3D))

        # Extract the bounds between which the object's dimensions are needed
        # It is necessary for this demo that the object's length and breath is smaller than that of the chessboard
        chessboard_points_cumulative_3d = np.delete(
            chessboard_points_cumulative_3d, 0, 1)
        roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)

        print(
            "Calibration completed... \nPlace the box in the field of view of the devices..."
        )
        """
                2: Measurement and display
                Measure the dimension of the object using depth maps from multiple RealSense devices
                The information from Phase 1 will be used here

                """

        # Enable the emitter of the devices
        device_manager.enable_emitter(True)

        # Load the JSON settings file in order to enable High Accuracy preset for the realsense
        device_manager.load_settings_json("./HighResHighAccuracyPreset.json")

        # Get the extrinsics of the device to be used later
        extrinsics_devices = device_manager.get_depth_to_color_extrinsics(
            frames)

        # Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image
        calibration_info_devices = defaultdict(list)
        for calibration_info in (transformation_devices, intrinsics_devices,
                                 extrinsics_devices):
            for key, value in calibration_info.items():
                calibration_info_devices[key].append(value)

        # Continue acquisition until terminated with Ctrl+C by the user
        while 1:
            # Get the frames from all the devices
            frames_devices = device_manager.poll_frames()

            # Calculate the pointcloud using the depth frames from all the devices
            point_cloud = calculate_cumulative_pointcloud(
                frames_devices, calibration_info_devices, roi_2D)

            # Get the bounding box for the pointcloud in image coordinates of the color imager
            bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(
                point_cloud, calibration_info_devices)

            # Draw the bounding box points on the color image and visualise the results
            visualise_measurements(frames_devices,
                                   bounding_box_points_color_image, length,
                                   width, height)

    except KeyboardInterrupt:
        print("The program was interupted by the user. Closing the program...")

    finally:
        device_manager.disable_streams()
        cv2.destroyAllWindows()
Ejemplo n.º 8
0
def collectTestingX():
    # Create a pipeline
    pipeline = rs.pipeline()

    #Create a config and configure the pipeline to stream
    #  different resolutions of color and depth streams
    config = rs.config()
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

    # Start streaming
    profile = pipeline.start(config)

    # Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()

    # We will be removing the background of objects more than
    #  clipping_distance_in_meters meters away
    clipping_distance_in_meters = 1  #1 meter
    clipping_distance = clipping_distance_in_meters / depth_scale

    # Create an align object
    # rs.align allows us to perform alignment of depth frames to others frames
    # The "align_to" is the stream type to which we plan to align depth frames.
    align_to = rs.stream.color
    align = rs.align(align_to)

    # Get frameset of color and depth
    frames = pipeline.wait_for_frames()
    # frames.get_depth_frame() is a 640x360 depth image

    # Align the depth frame to color frame
    aligned_frames = align.process(frames)

    # Get aligned frames
    aligned_depth_frame = aligned_frames.get_depth_frame(
    )  # aligned_depth_frame is a 640x480 depth image
    color_frame = aligned_frames.get_color_frame()

    depth_image = np.asanyarray(aligned_depth_frame.get_data())

    depth_image = removeBackground(depth_image)

    depth = frames.get_depth_frame()
    closest = 10000

    for y in range(480):
        for x in range(640):
            curr = depth.get_distance(x, y)
            closest = curr if curr < closest and curr != 0 else closest

    color_image = np.asanyarray(color_frame.get_data())

    # Remove background - Set pixels further than clipping_distance to grey
    grey_color = 153
    depth_image_3d = np.dstack(
        (depth_image, depth_image,
         depth_image))  #depth image is 1 channel, color is 3 channels
    bg_removed = np.where(
        (depth_image_3d > clipping_distance) | (depth_image_3d <= 0),
        grey_color, color_image)

    depth_colormap = cv2.applyColorMap(
        cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

    outputDataToFileStructure(depth_colormap, bg_removed)

    pipeline.stop()

    return (depth_colormap, closest)
Ejemplo n.º 9
0
def grid():
	d_pub = rospy.Publisher('depth_frame', Twist, queue_size=10)
	i_pub = rospy.Publisher('rgb_frame', Image, queue_size=10)

	rospy.init_node('talker', anonymous=True)
	rate = rospy.Rate(10) # 10hz

	try:
		pipeline = rs.pipeline()

		config = rs.config()
		config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
		config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 15)
		# Start streaming
		pipeline.start(config)

		# filters
		hole_filling = rs.hole_filling_filter()

		# get camera intrinsics
		profile = pipeline.get_active_profile()
		depth_sensor = profile.get_device().first_depth_sensor()
		depth_scale = depth_sensor.get_depth_scale()

		col_length = int(640*(1.0/8.0))
		row_length = int(480*(1.0/5.0))

		bridge = CvBridge()

		while not rospy.is_shutdown():
			# This call waits until a new coherent set of frames is available on a devicepip3 install opencv-python
			# Calls to get_frame_data(...) and get_frame_timestamp(...) on a device will return stable
			print('Getting frame data now')

			frames = pipeline.wait_for_frames()
			depth_frame = frames.get_depth_frame()
			color_frame = frames.get_color_frame()

			# depth_frame = hole_filling.process(depth_frame)
			if not depth_frame or not color_frame:
			    continue

			depth_image = np.asanyarray(depth_frame.get_data())
			rgb_image = np.asanyarray(color_frame.get_data())

			left_image = depth_image[ 2*row_length:3*row_length  , 1*col_length:2*col_length ]
			left_distances = depth_scale*left_image
			left_distances_filtered = left_distances[left_distances > 0]

			center_image = depth_image[ 2*row_length: 3*row_length, 3*col_length:5*col_length]
			center_distances = depth_scale*center_image
			center_distances_filtered = center_distances[center_distances > 0]

			right_image = depth_image[  2*row_length:3*row_length, 6*col_length:7*col_length ]
			right_distances = depth_scale*right_image
			right_distances_filtered = right_distances[right_distances > 0]
			# right_distances_projected = right_distances_filtered*math.sin(rad(42.6))

			mean = Twist()
			mean.linear.x = np.mean(left_distances_filtered)
			mean.linear.y = np.mean(center_distances_filtered)
			mean.linear.z = np.mean(right_distances_filtered)
			"""
			if DRAW_GRID:
			    color_image = np.asanyarray(color_frame.get_data())
			    # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
			    depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
			    # Stack both images horizontally
			    grid(color_image, w_portion, h_portion, 255, 255, 2)
			    images = np.hstack((color_image, depth_colormap))
			    cv2.imshow('RealSense', images)
			    cv2.waitKey(1)
			"""
			d_pub.publish(mean)
			i_pub.publish(bridge.cv2_to_imgmsg(rgb_image, "rgb8"))

			rate.sleep()

		pipeline.stop()


	except Exception as e:
		print('except : ', e)
		pass
Ejemplo n.º 10
0
def main():
    window_name = 'capture'
    result_name = 'perspective'
    USE_REAL_CAMERA = False

    try:
        # Configure depth and color streams
        pipeline = rs.pipeline()
        config = rs.config()

        if USE_REAL_CAMERA:
            config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
            config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        else:
            config.enable_device_from_file(BAG_FILE)

        # Start streaming
        pipeline.start(config)

        cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)
        cv2.moveWindow(window_name, 0, 0)
        cv2.namedWindow(result_name, cv2.WINDOW_AUTOSIZE)
        cv2.moveWindow(result_name, 640, 0)

        cnt = 0

        while True:
            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()
            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            if not depth_frame or not color_frame:
                continue

            # Convert images to numpy arrays
            color_image = np.asanyarray(color_frame.get_data())
            #depth_image = np.asanyarray(depth_frame.get_data())
            # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
            #depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

            # Grab new intrinsics (may be changed by decimation)
            depth_intrinsics = rs.video_stream_profile(depth_frame.profile).get_intrinsics()


            original = color_image.copy()
            color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
            ret, crnrs = cv2.findChessboardCorners(color_image, SIZE, cv2.CALIB_CB_FAST_CHECK)
            #cv2.drawChessboardCorners(color_image, SIZE, crnrs, ret)

            # draw point of corners
            if ret:
                fourc = ck.get_poi(crnrs)
                ck.draw_poi(color_image, fourc)
                fourxyz = get_poi_rs_dist(fourc, depth_frame, depth_intrinsics)
                msg0 = "{:.2f} x {:.2f} mm".format(ck.px_3d_dist(fourxyz[0], fourxyz[1]), ck.px_3d_dist(fourxyz[1], fourxyz[2]))
                ck.put_mytext(color_image, msg0)

            # perfrom warp perspective distortion
            if ret:
                cppts = ck.get_ppts(crnrs)
                M = ck.get_perspective_mat(cppts, DST_W, DST_H)
                perspective = cv2.warpPerspective(color_image, M, (DST_W, DST_H), cv2.INTER_LINEAR)
                cv2.imshow(result_name, perspective)


            if False and ret:
                p0 = crnrs[0][0]
                p1 = crnrs[1][0]
                print("p0:{} p1:{}".format(p0, p1))
                #dist_from_rs = depth_frame.get_distance(int(marker[0][0]), int(marker[0][1]))
                dist_from_rs = depth_frame.get_distance(p0[0], p0[1])
                dist_from_rs *= 1000

                #print('marker:{} dist_rs:{}'.format(marker, dist_from_rs))
                # cv2.putText(color_image, "%4.0fmm" % dist,
                #             (color_image.shape[1] - 500, color_image.shape[0] - 60),
                #             cv2.FONT_HERSHEY_SIMPLEX,
                #             1.2, (0, 255, 0), 3)

                cv2.putText(color_image, "%4dmm" % dist_from_rs,
                            (color_image.shape[1] - 500, color_image.shape[0] - 20),
                            cv2.FONT_HERSHEY_SIMPLEX,
                            1.1, (0, 255, 255), 3)

            # show a frame
            cv2.imshow(window_name, color_image)
            key = cv2.waitKey(1)
            if key & 0xFF == ord('q') or key == 0x1B:
                break
            elif key & 0xFF == ord('s'):
                fn = 'result-{:02d}.png'.format(cnt)
                print('save result to {}'.format(fn))
                cv2.imwrite(fn, color_image)
                cv2.imwrite('origin-{:02d}.png'.format(cnt), original)
                cnt += 1
            elif key == 0x20:
                cv2.waitKey(0)
    finally:
        cv2.destroyAllWindows()
        # Stop streaming
        pipeline.stop()
import math
import time
import pyrealsense2 as rs
pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)
pipe.start(cfg)


def quaternion_to_euler(x, y, z, w):
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    X = math.degrees(math.atan2(t0, t1))
    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    Y = math.degrees(math.asin(t2))
    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    Z = math.degrees(math.atan2(t3, t4))
    return X, Y, Z


while True:
    frames = pipe.wait_for_frames()
    pose = frames.get_pose_frame()
    if pose:
        data = pose.get_pose_data()
        Alpha, Beta, Gamma = quaternion_to_euler(data.rotation.x,
                                                 data.rotation.y,
                                                 data.rotation.z,
Ejemplo n.º 12
0
def detect():
    source, weights, view_img, imgsz,save_vidio,detect_image,detect_vidio,webcam,dist_thres_lim,savevid_f = opt.source, opt.weights, opt.view_img,\
                                                                          opt.img_size,opt.save_vidio,opt.detect_img,opt.detect_vidio,\
                                                                                                  opt.detect_webcam,opt.dist_thres_lim,opt.savevid_f
    set_logging()
    device = select_device(opt.device)
    half = device.type != 'cpu'  # half precision only supported on CUDA
    # Load model
    model = attempt_load(weights, map_location=device)  # load FP33 model
    stride = int(model.stride.max())  # model stride
    imgsz = check_img_size(imgsz, s=stride)  # check img_size
    if half:
        model.half()  # to FP16
    pipe = rs.pipeline()
    cfg = rs.config()
    if not webcam:
        cfg.enable_device_from_file(file_name=source)
    cfg.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30)
    cfg.enable_stream(rs.stream.color, 848, 480, rs.format.rgb8, 30)
    pipe.start(cfg)
    a = 0
    names = model.module.names if hasattr(model, 'module') else model.names
    colors = [[random.randint(0, 255) for _ in range(3)] for _ in names]
    # Run inference
    if device.type != 'cpu':
        model(
            torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(
                next(model.parameters())))  # run once
    start = time.time()
    colorizer = rs.colorizer()
    hole_filling = rs.hole_filling_filter()
    try:
        while True:
            if detect_image:
                im0s, depth, colorized_depth = detects_img(
                    pipe, hole_filling, colorizer)
            if detect_vidio:
                im0s, depth, colorized_depth = detects_vidio(
                    pipe, hole_filling, colorizer)
            img = read_image(im0s)
            img = torch.from_numpy(img).to(device)
            img = img.half() if half else img.float()  # uint8 to fp16/32
            img /= 255.0  # 0 - 255 to 0.0 - 1.0
            if img.ndimension() == 3:
                img = img.unsqueeze(0)
                # Inference
                # t1 = time_synchronized()
                pred = model(img, augment=opt.augment)[0]
                # Apply NMS
                pred = non_max_suppression(pred,
                                           opt.conf_thres,
                                           opt.iou_thres,
                                           classes=opt.classes,
                                           agnostic=opt.agnostic_nms)

                # all_coords = []
            # t3 = time.time()
            for _, det in enumerate(pred):
                if len(det):
                    det = det.cpu()
                    # Rescale boxes from img_size to im0 size
                    det[:, :4] = scale_coords(img.shape[2:], det[:, :4],
                                              im0s.shape).round()
                for *xyxy, conf, cls in reversed(det):
                    depth_temp = depth[int(xyxy[1]):int(xyxy[3]),
                                       int(xyxy[0]):int(xyxy[2])].astype(float)
                    depth_temp = depth_temp * 0.001
                    dist, _, _, _ = cv2.mean(depth_temp)
                    dist_temp = Decimal(dist).quantize(Decimal('0.000'))
                    y_mid = (int(xyxy[1]) + int(xyxy[3])) / 2
                    x_mid = (xyxy[0] + xyxy[2]) / 2
                    len_x = np.cumsum(
                        depth[int(y_mid),
                              int(xyxy[0]):int(xyxy[2])])[-1] * 0.00165
                    len_y = np.cumsum(depth[int(xyxy[1]):int(xyxy[3]),
                                            int(x_mid)])[-1] * 0.00165
                    label1 = str(dist_temp) + "m" + str(
                        int(len_x)) + "mm" + ',' + str(int(len_y)) + "mm"
                    # all_coords.append(xyxy)
                    label = f'{names[int(cls)]} {conf:.2f}'
                    plot_one_box(xyxy,
                                 im0s,
                                 label=label,
                                 color=colors[int(cls)],
                                 line_thickness=3)
                    # plot_dots_on_people(xyxy, im0s)
                    plot_one_box(xyxy,
                                 colorized_depth,
                                 label=label1,
                                 color=colors[int(cls)],
                                 line_thickness=2)
                    # distancing_all(all_coords, im0s,depth=depth,dist_thres_lim=dist_thres_lim)

            images = np.hstack((cv2.cvtColor(im0s, cv2.COLOR_RGB2BGR),
                                cv2.cvtColor(colorized_depth,
                                             cv2.COLOR_RGB2BGR)))
            a += 1
            # if a %1 ==0:
            print("\r>>>FPS:{:.2f}<<<     ".format(a / (time.time() - start)),
                  end="")
            if view_img:
                cv2.imshow("RealSense", images)
                # t6 = time.time()
                # print("cv2imshow()  time :" + str(t6 - t4))
                # 按下“q”键停止q
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    # cv2.destroyAllWindows()
                    break
            if detect_image:
                cv2.imwrite("out_img.png", images)
                break
            if save_vidio:
                # if  save_vidio and a!=savevid_f:
                if a != savevid_f:
                    fps, w, h = 30, images.shape[1], images.shape[0]
                    # save_path += '.mp4'
                    if a == 1:
                        vid_writer = cv2.VideoWriter(
                            "test.mp4", cv2.VideoWriter_fourcc(*'mp4v'), fps,
                            (w, h))
                    vid_writer.write(images)
                    # print("\r video [{}/{}] save path{} ".format(a,300,save_vidio),end="")
                else:
                    print(" Done,vidio save to{}, time{:.2f}".format(
                        "./test.mp4",
                        time.time() - start))
                    # vid_writer.release()
                    cv2.destroyAllWindows()
                    break
            # t7 =time.time()
            # print('cv2.waitKey()' + str(t7 - t6))

    finally:
        # pipe.stop()
        pass
Ejemplo n.º 13
0
    #         print(util.serial_number(pipeline.start(config).get_device()))
    #
    #     show_multi_cam(pipelines)
    #
    # finally:
    #     # Start streaming from both cameras
    #     for pipeline, _ in pipelines:
    #         pipeline.stop()

    # capture_one_cam(cams_ids[0])

    pc: rs2.pointcloud = rs2.pointcloud()
    # points = rs2.points()

    pipe: rs2.pipeline = rs2.pipeline()
    config: rs2.config = rs2.config()
    config.enable_stream(rs2.stream.color, 640, 480, rs2.format.rgb8, 30)
    config.enable_stream(rs2.stream.depth, 640, 480, rs2.format.z16, 30)
    pipe.start(config)
    while True:
        frames: rs2.composite_frame = pipe.wait_for_frames()
        color: rs2.video_frame = frames.get_color_frame()
        depth: rs2.depth_frame = frames.get_depth_frame()
        if (not depth) or (not color):
            continue

        points: rs2.points = pc.calculate(depth)
        color = frames.get_color_frame()
        pc.map_to(color)

        vertices = np.asanyarray(points.get_vertices())
import cv2
import time

# check the number of connected devices
devices_ = rs.context().query_devices()
print('Found {} connected devices'.format(len(devices_)))

serial_numbers = []
for dev_ in devices_:
    serial_numbers.append(dev_.get_info(rs.camera_info.serial_number))

pipelines = []
configs = []
for idx in range(len(devices_)):
    pipelines.append(rs.pipeline())
    configs.append(rs.config())
    configs[idx].enable_device(serial_numbers[idx])
    configs[idx].enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    pipelines[idx].start(configs[idx])

save_image = False
current_idx = None
try:
    while True:
        for idx in range(len(devices_)):
            current_idx = idx
            frames = pipelines[idx].wait_for_frames()
            depth_frame = frames.get_depth_frame()
            if not depth_frame:
                print('No data received')
                continue
def camThread(LABELS, results, frameBuffer, camera_mode, camera_width,
              camera_height, background_transparent_mode, background_img):
    global fps
    global detectfps
    global lastresults
    global framecount
    global detectframecount
    global time1
    global time2
    global cam
    global window_name
    global depth_scale
    global align_to
    global align

    # Configure depth and color streams
    #  Or
    # Open USB Camera streams
    if camera_mode == 0:
        pipeline = rs.pipeline()
        config = rs.config()
        config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        profile = pipeline.start(config)
        depth_sensor = profile.get_device().first_depth_sensor()
        depth_scale = depth_sensor.get_depth_scale()
        align_to = rs.stream.color
        align = rs.align(align_to)
        window_name = "RealSense"
    elif camera_mode == 1:
        cam = cv2.VideoCapture(0)
        if cam.isOpened() != True:
            print("USB Camera Open Error!!!")
            sys.exit(0)
        cam.set(cv2.CAP_PROP_FPS, 30)
        cam.set(cv2.CAP_PROP_FRAME_WIDTH, camera_width)
        cam.set(cv2.CAP_PROP_FRAME_HEIGHT, camera_height)
        window_name = "USB Camera"

    cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE)

    while True:
        t1 = time.perf_counter()

        # 0:= RealSense Mode
        # 1:= USB Camera Mode

        if camera_mode == 0:
            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()
            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            if not depth_frame or not color_frame:
                continue
            if frameBuffer.full():
                frameBuffer.get()
            color_image = np.asanyarray(color_frame.get_data())

        elif camera_mode == 1:
            # USB Camera Stream Read
            s, color_image = cam.read()
            if not s:
                continue
            if frameBuffer.full():
                frameBuffer.get()
            frames = color_image

        height = color_image.shape[0]
        width = color_image.shape[1]
        frameBuffer.put(color_image.copy())
        res = None

        if not results.empty():
            res = results.get(False)
            detectframecount += 1
            imdraw = overlay_on_image(frames,
                                      res,
                                      LABELS,
                                      camera_mode,
                                      background_transparent_mode,
                                      background_img,
                                      depth_scale=depth_scale,
                                      align=align)
            lastresults = res
        else:
            imdraw = overlay_on_image(frames,
                                      lastresults,
                                      LABELS,
                                      camera_mode,
                                      background_transparent_mode,
                                      background_img,
                                      depth_scale=depth_scale,
                                      align=align)

        cv2.imshow(window_name, cv2.resize(imdraw, (width, height)))

        if cv2.waitKey(1) & 0xFF == ord('q'):
            # Stop streaming
            if pipeline != None:
                pipeline.stop()
            sys.exit(0)

        ## Print FPS
        framecount += 1
        if framecount >= 15:
            fps = "(Playback) {:.1f} FPS".format(time1 / 15)
            detectfps = "(Detection) {:.1f} FPS".format(detectframecount /
                                                        time2)
            framecount = 0
            detectframecount = 0
            time1 = 0
            time2 = 0
        t2 = time.perf_counter()
        elapsedTime = t2 - t1
        time1 += 1 / elapsedTime
        time2 += elapsedTime
def run_demo():
	
	# Define some constants 
	resolution_width = 1280 # pixels
	resolution_height = 720 # pixels
	frame_rate = 15  # fps
	dispose_frames_for_stablisation = 30  # frames
	
	chessboard_width = 6 # squares
	chessboard_height = 9 	# squares
	square_size = 0.0253 # meters

	try:
		# Enable the streams from all the intel realsense devices
		rs_config = rs.config()
		rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
		rs_config.enable_stream(rs.stream.infrared, 1, resolution_width, resolution_height, rs.format.y8, frame_rate)
		rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)

		# Use the device manager class to enable the devices and get the frames
		device_manager = DeviceManager(rs.context(), rs_config)
		device_manager.enable_all_devices()
		
		# Allow some frames for the auto-exposure controller to stablise
		for frame in range(dispose_frames_for_stablisation):
			frames = device_manager.poll_frames()

		assert( len(device_manager._available_devices) > 0 )
		"""
		1: Calibration
		Calibrate all the available devices to the world co-ordinates.
		For this purpose, a chessboard printout for use with opencv based calibration process is needed.

		"""
		# Get the intrinsics of the realsense device 
		intrinsics_devices = device_manager.get_device_intrinsics(frames)
		
	 # Set the chessboard parameters for calibration 
		chessboard_params = [chessboard_height, chessboard_width, square_size] 
		
		# Estimate the pose of the chessboard in the world coordinate using the Kabsch Method
		calibrated_device_count = 0
		while calibrated_device_count < len(device_manager._available_devices):
			frames = device_manager.poll_frames()
			pose_estimator = PoseEstimation(frames, intrinsics_devices, chessboard_params)
			transformation_result_kabsch  = pose_estimator.perform_pose_estimation()
			object_point = pose_estimator.get_chessboard_corners_in3d()
			calibrated_device_count = 0
			for device in device_manager._available_devices:
				if not transformation_result_kabsch[device][0]:
					print("Place the chessboard on the plane where the object needs to be detected..")
				else:
					calibrated_device_count += 1

		# Save the transformation object for all devices in an array to use for measurements
		transformation_devices={}
		chessboard_points_cumulative_3d = np.array([-1,-1,-1]).transpose()
		for device in device_manager._available_devices:
			transformation_devices[device] = transformation_result_kabsch[device][1].inverse()
			points3D = object_point[device][2][:,object_point[device][3]]
			points3D = transformation_devices[device].apply_transformation(points3D)
			chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d,points3D) )

		# Extract the bounds between which the object's dimensions are needed
		# 	It is necessary for this demo that the object's length and breath is smaller than that of the chessboard
		chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1)
		roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)

		print("Calibration completed... \nPlace the box in the field of view of the devices...")


		"""
		2: Measurement and display
		Measure the dimension of the object using depth maps from multiple RealSense devices
		The information from Phase 1 will be used here

		"""

		# Enable the emitter of the devices
		device_manager.enable_emitter(True)

		# Load the JSON settings file in order to enable High Accuracy preset for the realsense
		device_manager.load_settings_json("./HighResHighAccuracyPreset.json")

		# Get the extrinsics of the device to be used later
		extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames)

		# Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image
		calibration_info_devices = defaultdict(list)
		for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices):
			for key, value in calibration_info.items():
				calibration_info_devices[key].append(value)

		# Continue acquisition until terminated with Ctrl+C by the user
		while 1:
			 # Get the frames from all the devices
				frames_devices = device_manager.poll_frames()

				# Calculate the pointcloud using the depth frames from all the devices
				point_cloud = calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2D)

				# Get the bounding box for the pointcloud in image coordinates of the color imager
				bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(point_cloud, calibration_info_devices )

				# Draw the bounding box points on the color image and visualise the results
				visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height)

	except KeyboardInterrupt:
		print("The program was interupted by the user. Closing the program...")
	
	finally:
		device_manager.disable_streams()
		cv2.destroyAllWindows()
Ejemplo n.º 17
0
    def __init__(self):
        camera_id = str(rospy.get_param('~camera_id'))
        color_width = rospy.get_param('~color_width')
        color_high = rospy.get_param('~color_high')
        charuco_row = rospy.get_param('~charuco_row')
        charuco_col = rospy.get_param('~charuco_col')
        square_length = rospy.get_param('~square_length')
        marker_length = rospy.get_param('~marker_length')
        self.cnd = 0
        self.frameId = 0
        self.pipeline = rs.pipeline()
        rs_config = rs.config()
        rs_config.enable_stream(rs.stream.color, color_width, color_high,
                                rs.format.bgr8, 30)
        self.pipeline.start(rs_config)

        # Constant parameters used in Aruco methods
        self.aruco_param = aruco.DetectorParameters_create()
        self.aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
        # Create grid board object we're using in our stream
        self.charuco_board = aruco.CharucoBoard_create(
            squaresX=charuco_col,
            squaresY=charuco_row,
            squareLength=square_length,
            markerLength=marker_length,
            dictionary=self.aruco_dict)

        # Check for camera calibration data

        config = ConfigParser.ConfigParser()
        config.optionxform = str
        rospack = rospkg.RosPack()
        self.curr_path = rospack.get_path('hand_eye')
        config.read(self.curr_path + '/config/camera_' + str(camera_id) +
                    '_internal.ini')
        internal_name = 'Internal_' + str(color_width) + '_' + str(color_high)
        b00 = float(config.get(internal_name, "Key_1_1"))
        b01 = float(config.get(internal_name, "Key_1_2"))
        b02 = float(config.get(internal_name, "Key_1_3"))
        b10 = float(config.get(internal_name, "Key_2_1"))
        b11 = float(config.get(internal_name, "Key_2_2"))
        b12 = float(config.get(internal_name, "Key_2_3"))
        b20 = float(config.get(internal_name, "Key_3_1"))
        b21 = float(config.get(internal_name, "Key_3_2"))
        b22 = float(config.get(internal_name, "Key_3_3"))

        self.cameraMatrix = np.mat([[b00, b01, b02], [b10, b11, b12],
                                    [b20, b21, b22]])
        # c_x = 643.47548083
        # c_y = 363.67742746
        # f_x = 906.60886808
        # f_y = 909.34831447
        # k_1 = 0.16962942
        # k_2 = -0.5560001
        # p_1 = 0.00116353
        # p_2 = -0.00122694
        # k_3 = 0.52491878

        # c_x = 649.007507324219
        # c_y = 356.122222900391
        # f_x = 922.76806640625
        # f_y = 923.262023925781

        # self.cameraMatrix = np.array([[f_x, 0, c_x],
        #                        [0, f_y, c_y],
        #                        [0, 0, 1]])
        # self.distCoeffs = np.array([k_1, k_2, p_1, p_2, k_3])
        self.distCoeffs = np.array([0.0, 0, 0, 0, 0])

        # Create vectors we'll be using for rotations and translations for postures
        self.rvecs = None
        self.tvecs = None
        self.rvecs_arr = np.zeros((3, NUMBER))
        self.tvecs_arr = np.zeros((3, NUMBER))

        self.cam = None
        self.QueryImg = None
        self.init_server()
        frames = self.pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        self.QueryImg = np.asanyarray(color_frame.get_data())
Ejemplo n.º 18
0
def create_pipeline(config):
    """Sets up the pipeline to extract depth and rgb frames

    Arguments:
        config {dict} -- A dictionary mapping for configuration. see default.yaml

    Returns:
        tuple -- pipeline, pointcloud, decimate, filters(list), colorizer (not used)
    """

    # Ensure device is connected
    ctx = rs.context()
    devices = ctx.query_devices()
    if len(devices) == 0:
        logging.error("No connected Intel Realsense Device!")
        sys.exit(1)

    if config['advanced']:
        logging.info(
            "Attempting to enter advanced mode and upload JSON settings file")
        load_setting_file(ctx, devices, config['advanced'])

    # Configure streams
    pipeline = rs.pipeline()
    rs_config = rs.config()
    rs_config.enable_stream(rs.stream.depth, config['depth']['width'],
                            config['depth']['height'], rs.format.z16,
                            config['depth']['framerate'])
    # other_stream, other_format = rs.stream.infrared, rs.format.y8
    rs_config.enable_stream(rs.stream.color, config['color']['width'],
                            config['color']['height'], rs.format.rgb8,
                            config['color']['framerate'])

    # Start streaming
    pipeline.start(rs_config)
    profile = pipeline.get_active_profile()

    # Processing blocks
    filters = []
    decimate = None
    align = rs.align(rs.stream.color)
    depth_to_disparity = rs.disparity_transform(True)
    disparity_to_depth = rs.disparity_transform(False)
    # Decimation
    if config.get("filters").get("decimation"):
        filt = config.get("filters").get("decimation")
        if filt.get('active', True):
            filt.pop('active', None)  # Remove active key before passing params
            decimate = rs.decimation_filter(**filt)

    # Spatial
    if config.get("filters").get("spatial"):
        filt = config.get("filters").get("spatial")
        if filt.get('active', True):
            filt.pop('active', None)  # Remove active key before passing params
            my_filter = rs.spatial_filter(**filt)
            filters.append(my_filter)

    # Temporal
    if config.get("filters").get("temporal"):
        filt = config.get("filters").get("temporal")
        if filt.get('active', True):
            filt.pop('active', None)  # Remove active key before passing params
            my_filter = rs.temporal_filter(**filt)
            filters.append(my_filter)

    process_modules = (align, depth_to_disparity, disparity_to_depth, decimate)
    # Create colorizer and point cloud
    # colorizer = rs.colorizer(2)
    pc = rs.pointcloud()

    intrinsics = get_intrinsics(pipeline, rs.stream.color)
    proj_mat = create_projection_matrix(intrinsics)

    return pipeline, pc, process_modules, filters, proj_mat
def realsense_behaviour():
    # set show_depth to True to show the rgb and depth frames together
    show_depth = True

    # realsense min and max distance
    minDist = 0.3
    maxDist = 4.5

    # Configure depth and color streams
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

    # Start streaming
    profile = pipeline.start(config)

    # Show images
    cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)

    try:
        while True:
            # skip 5 first frames to give Auto-Exposure time to adjust
            for i in range(5):
                pipeline.wait_for_frames()

            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()
            color_frame = frames.get_color_frame()
            depth_frame = frames.get_depth_frame()

            # Access color component of the frame
            color = np.asanyarray(color_frame.get_data())

            # Access depth component
            colorizer = rs.colorizer()
            colorizer_depth = np.asanyarray(
                colorizer.colorize(depth_frame).get_data())

            # Depth and rgb image alignment
            align = rs.align(rs.stream.color)
            frames = align.process(frames)
            aligned_depth_frame = frames.get_depth_frame()
            colorized_depth = np.asanyarray(
                colorizer.colorize(aligned_depth_frame).get_data())

            valX = np.array([30, 319, 609])
            valY = np.array([30, 239, 449])
            dist = np.zeros((3, 3))
            valid_matrix = np.zeros((3, 3))

            for xCount in range(0, 3):
                for yCount in range(0, 3):
                    distance = aligned_depth_frame.get_distance(
                        valX[xCount], valY[yCount])
                    if (distance < 0.3):
                        distance = np.inf
                        valid_matrix[yCount, xCount] = 0
                    else:
                        valid_matrix[yCount, xCount] = 1
                    dist[yCount, xCount] = distance
                    cv2.circle(color, (valX[xCount], valY[yCount]), 5,
                               (0, 0, 255), 2)
                    cv2.circle(colorized_depth, (valX[xCount], valY[yCount]),
                               5, (0, 255, 0), 2)
            # print("\033c")
            # print(dist)

            # Find index of valid  values
            validIdxL = np.nonzero(valid_matrix[0:3, 0])
            validIdxR = np.nonzero(valid_matrix[0:3, 2])

            if (((len(validIdxL[0])) > 1 and len(validIdxR[0])) > 1):
                angle = (np.mean(dist[validIdxL, 0]) -
                         np.mean(dist[validIdxR, 2])) * 180 / np.pi
                value = 1 / np.amin(dist)
                # saturation
                if value > 1 / minDist:  # saturation to max speed
                    value = 3.
                elif value < 1 / maxDist:  # saturation to min speed
                    value = 0
            else:
                angle = 0
                value = 0

            # print(angle, value)
            vect = [angle, -value]

            # Fill the rs_queue
            global_vars.queue_rs.put(vect)

            # Show the two frames together:
            if show_depth:
                images = np.hstack((color, colorized_depth))
            else:
                images = color

            cv2.imshow('RealSense', images)
            k = cv2.waitKey(1) & 0xFF
            if k == ord('q'):
                break

            # Create a vector based on depth data points

    finally:

        # Stop streaming
        pipeline.stop()
    def disable_streams(self):
        self._config.disable_all_streams()


"""
  _____           _    _               
 |_   _|___  ___ | |_ (_) _ __    __ _ 
   | | / _ \/ __|| __|| || '_ \  / _` |
   | ||  __/\__ \| |_ | || | | || (_| |
   |_| \___||___/ \__||_||_| |_| \__, |
                                  |___/ 

"""
if __name__ == "__main__":
    try:
        c = rs.config()
        c.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)
        c.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 6)
        c.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 6)
        c.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 6)
        device_manager = DeviceManager(rs.context(), c)
        device_manager.enable_all_devices()
        for k in range(150):
            frames = device_manager.poll_frames()
        device_manager.enable_emitter(True)
        device_extrinsics = device_manager.get_depth_to_color_extrinsics(
            frames)
    finally:
        device_manager.disable_streams()
Ejemplo n.º 21
0
    return file_paths
 
# import pcl
# from pcl import pcl_visualization

CV2_LBUTTON_FLAG = False

# Configure depth and color streams...
# ...from Camera 1
pipelines = []
configs = []
profiles = []


pipeline_1 = rs.pipeline()
config_1 = rs.config()
config_1.enable_device('816612061111')
config_1.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config_1.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# ...from Camera 2
pipeline_2 = rs.pipeline()
config_2 = rs.config()
config_2.enable_device('816612061344')
config_2.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config_2.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)


# Start streaming from both cameras
pipeline_1.start(config_1)
pipeline_2.start(config_2)
def main():

    ctx = rs.context()
    devices = ctx.query_devices()
    device = devices[0]
    # print device info
    #print('advanced_mode: %s' % device.get_info(rs.camera_info.advanced_mode));
    print('asic_serial_number: %s' %
          device.get_info(rs.camera_info.asic_serial_number))
    print('camera_locked: %s' % device.get_info(rs.camera_info.camera_locked))
    print('debug_op_code: %s' % device.get_info(rs.camera_info.debug_op_code))
    print('firmware_update_id: %s' %
          device.get_info(rs.camera_info.firmware_update_id))
    print('firmware_version: %s' %
          device.get_info(rs.camera_info.firmware_version))
    print('name: %s' % device.get_info(rs.camera_info.name))
    print('physical_port: %s' % device.get_info(rs.camera_info.physical_port))
    print('product_id: %s' % device.get_info(rs.camera_info.product_id))
    print('product_line: %s' % device.get_info(rs.camera_info.product_line))
    #print('recommended_firmware_version: %s' % device.get_info(rs.camera_info.recommended_firmware_version));
    print('serial_number: %s' % device.get_info(rs.camera_info.serial_number))
    #print('usb_type_descriptor: %s' % device.get_info(rs.camera_info.usb_type_descriptor));

    filters = dict()
    filters['align'] = rs.align(rs.stream.color)
    filters['spatial'] = rs.spatial_filter()
    filters['spatial'].set_option(rs.option.filter_magnitude, 5)
    filters['spatial'].set_option(rs.option.filter_smooth_alpha, 1)
    filters['spatial'].set_option(rs.option.filter_smooth_delta, 50)
    filters['spatial'].set_option(rs.option.holes_fill, 3)
    filters['temporal'] = rs.temporal_filter()
    filters['hole'] = rs.hole_filling_filter()
    filters['disparity'] = rs.disparity_transform(True)
    filters['depth'] = rs.disparity_transform(False)
    config = rs.config()
    config.enable_device(device.get_info(rs.camera_info.serial_number))
    config.enable_stream(rs.stream.depth, IMG_WIDTH, IMG_HEIGHT, rs.format.z16,
                         30)
    config.enable_stream(rs.stream.color, IMG_WIDTH, IMG_HEIGHT,
                         rs.format.bgr8, 30)
    pipeline = rs.pipeline()
    if exists('captured'): rmtree('captured')
    if exists('images'): rmtree('images')
    mkdir('captured')
    mkdir('images')
    sequence = 0
    while True:
        ch = input('press to capture')
        pipeline.start(config)
        for i in range(5):
            pipeline.wait_for_frames()
        frames = pipeline.wait_for_frames()
        alignment = filters['align'].process(frames)
        depth_frame = alignment.get_depth_frame()
        #depth_frame = filters['disparity'].process(depth_frame);
        #depth_frame = filters['spatial'].process(depth_frame);
        #depth_frame = filters['temporal'].process(depth_frame);
        #depth_frame = filters['depth'].process(depth_frame);
        depth_frame = filters['hole'].process(depth_frame)
        color_frame = alignment.get_color_frame()
        if not depth_frame or not color_frame:
            print("failed to capture!")
            continue
        depth_image = np.asanyarray(depth_frame.get_data())
        print(np.min(depth_image), np.max(depth_image))
        mask = masked(depth_image.copy())
        color_image = np.asanyarray(color_frame.get_data())
        cv2.imwrite(
            join('captured',
                 str(sequence).zfill(3) + '_mask.png'),
            cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03),
                              cv2.COLORMAP_JET))
        cv2.imwrite(join('captured',
                         str(sequence).zfill(3) + '.png'), color_image)
        masked_color = color_image.copy()
        masked_color[mask == CLIPPED_HIGH] = np.zeros((color_image.shape[-1]))
        cv2.imwrite(join('images',
                         str(sequence).zfill(3) + '.png'), masked_color)
        pipeline.stop()
        sequence += 1
Ejemplo n.º 23
0
    model.load_weights(weightsfile)

    model.net_info["height"] = 160
    inp_dim = int(model.net_info["height"])

    assert inp_dim % 32 == 0
    assert inp_dim > 32

    if CUDA:
        model.cuda()

    model.eval()

    # Setup Realsense pipeline
    pipe = rs.pipeline()
    configure = rs.config()
    width = 640; height = 480;
    configure.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30)
    configure.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30)
    dec_filter = rs.decimation_filter ()   # Decimation - reduces depth frame density
    spat_filter = rs.spatial_filter()      # Spatial    - edge-preserving spatial smoothing
    temp_filter = rs.temporal_filter()    # Temporal   - reduces temporal noise
    pipe.start(configure)
    align_to = rs.stream.color
    align = rs.align(align_to)

    while(1):

        # temp = pipe.wait_for_frames()
        # aligned_frames = align.process(temp)
        # depth_frame = aligned_frames.get_depth_frame()
Ejemplo n.º 24
0
def manager():
    options = {
        'model': 'cfg/yolo.cfg',
        'load': 'bin/yolo.weights',
        'threshold': 0.5,
        'gpu': 0.5
    }

    tfnet = TFNet(options)

    pipe_mngr = rs.pipeline()
    config = rs.config()
    profile = pipe_mngr.start(config)

    frames_mngr = pipe_mngr.wait_for_frames()
    color_mngr = frames_mngr.get_color_frame()
    depth_mngr = frames_mngr.get_depth_frame()

    capture_mngr = cv2.VideoCapture(0)
    capture_mngr.set(cv2.CAP_PROP_FRAME_WIDTH, 480)
    capture_mngr.set(cv2.CAP_PROP_FRAME_HEIGHT, 640)

    for p in range(0, 5):
        time.sleep(1)
        print(p + 1)

    i = 0
    iteration = 0

    align_to = rs.stream.color
    align = rs.align(align_to)

    while True:
        print("\nMODE IS: ", i)
        iteration += 1

        if i == 1:
            #time.sleep(0.25)
            if not depth_mngr: continue

            frames_mngr = pipe_mngr.wait_for_frames()
            depth_mngr = frames_mngr.get_depth_frame()
            align_to = rs.stream.color
            align = rs.align(align_to)

            color_f = frames_mngr.get_color_frame()
            colorr = np.asanyarray(color_f.get_data())

            i = realsense(depth_mngr, profile, frames_mngr, align)
            print("Iterations ", iteration)

        elif i == 0:
            #refresh depth output
            ########
            color_image = np.asanyarray(color_mngr.get_data())
            depth_image = np.asanyarray(depth_mngr.get_data())

            depth_colormap = cv2.applyColorMap(
                cv2.convertScaleAbs(depth_image, alpha=0.05), cv2.COLORMAP_JET)

            images = depth_colormap

            cv2.namedWindow('Manager', cv2.WINDOW_AUTOSIZE)
            cv2.imshow('Depth_RealSense', images)
            cv2.waitKey(1)
            ########

            i = yolo(capture_mngr, tfnet)
            print("Iterations ", iteration)

        elif i == 2:
            print("PRESSED 'Q'")
            print("Iterations ", iteration)
            break
Ejemplo n.º 25
0
def main():
    global handin
    global currentState

    # img_rows, img_cols, maxFrames = 32, 32, 100
    img_rows, img_cols, maxFrames = 50, 50, 55
    depthFrame = 0
    cameraHeightR = 480
    cameraWidthR = 848
    # cameraWidthR = 640
    frameRateR = 60
    # frameRateR = 30

    # crop parameter
    xupam = 350
    yupam = 200

    xdpam = 250
    ydpam = 300

    #1,3,4,5,8,9,12,13,14,15
    classGest = ['1', '12', '13', '14', '15', '3', '4', '5', '8', '9']
    nameGest = [
        'call', 'scroll up', 'scroll down', 'right', 'leftback', "like",
        'play/pause', 'close', 'click', 'down back'
    ]
    #classGest = ['11', '12', '13','4','8']
    #nameGest = ['back', 'scroll up', 'scroll down', 'close 4', 'click 8']
    delayGest = 5
    delayBol = False
    backgroundRemove = True
    boxCounter = True

    # load the model and weight
    json_file = open('3dcnnresult/33/3dcnnmodel.json', 'r')
    loaded_model_json = json_file.read()
    json_file.close()
    loaded_model = model_from_json(loaded_model_json)

    # load weights into new model
    loaded_model.load_weights("3dcnnresult/33/3dcnnmodel.hd5")
    print("Loaded model from disk")

    loaded_model.compile(loss=categorical_crossentropy,
                         optimizer='rmsprop',
                         metrics=['accuracy'])

    conf = loaded_model.model.get_config()

    shapeInput, ap = loaded_model.model.get_layer(name="dense_2").input_shape
    shapeOutput, sp = loaded_model.model.get_layer(name="dense_2").output_shape
    weightDense2 = loaded_model.model.get_layer(
        name="dense_2").get_weights()[0]
    weightDense22I = loaded_model.model.get_layer(
        name="dense_2").get_weights()[1]
    weightDense22A = loaded_model.model.get_layer(
        name="dense_2").get_weights()[1]

    # load the FSM
    ytfsmfile = "youtube_fsm.txt"
    ytFsmTable = loadFSM(ytfsmfile)

    updatedWeight = False
    updatedWeight2 = False
    ''''
    print("========================New weight I ==================================")
    print(weightDense22I)
    print("========================New weight A ==================================")
    print(weightDense22A)
    '''

    #print(newWeight[0])

    #NweightDense2 = loaded_model.model.get_layer(name="dense_2").get_weights()[0]

    #modelConfig = loaded_model.get_config()
    #print(modelConfig)

    # setup cv face detection
    face_cascade = cv2.CascadeClassifier(
        'haarcascades/haarcascade_frontalface_alt2.xml')

    # setup Realsense
    # Configure depth and color streams
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, cameraWidthR, cameraHeightR,
                         rs.format.z16, frameRateR)
    config.enable_stream(rs.stream.color, cameraWidthR, cameraHeightR,
                         rs.format.bgr8, frameRateR)

    # if using background removal

    # Start streaming
    profile = pipeline.start(config)

    # Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()
    # print "Depth Scale is: " , depth_scale

    # We will be removing the background of objects more than
    #  clipping_distance_in_meters meters away
    clipping_distance_in_meters = 1  # 1 meter
    clipping_distance = clipping_distance_in_meters / depth_scale

    # for text purpose
    imgTxt = np.zeros((480, 400, 3), np.uint8)
    txt = "OpenCV"
    txtLoad = "["
    txtDelay = "["
    txtRecord = "Capture"
    txtDel = "Delay"
    txtProbability = "0%"
    font = cv2.FONT_HERSHEY_SIMPLEX

    framearray = []
    ctrDelay = 0
    channel = 1
    gestCatch = False
    gestStart = False

    startTime = 0
    endTime = 0

    x, y, w, h = 0, 0, 0, 0

    align_to = rs.stream.color
    align = rs.align(align_to)
    num_frames = 0
    im_width, im_height = (848, 480)
    # max number of hands we want to detect/track
    num_hands_detect = 1
    score_thresh = 0.37
    try:
        while True:

            dataImg = []

            # Wait for a coherent pair of frames: depth and color
            frames = pipeline.wait_for_frames()

            # Align the depth frame to color frame
            aligned_frames = align.process(frames)

            # Get aligned frames
            aligned_depth_frame = aligned_frames.get_depth_frame()
            color_frame = aligned_frames.get_color_frame()

            depth_image = np.asanyarray(aligned_depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())

            # Validate that both frames are valid
            if not aligned_depth_frame or not color_frame:
                continue

            #print(scores)

            num_frames += 1
            image_np = color_image
            try:
                image_np = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
            except:
                print("Error converting to RGB")
            boxes, scores = detector_utils.detect_objects(
                image_np, detection_graph, sess)
            for i in range(num_hands_detect):
                if (scores[i] > score_thresh):
                    handin = True

                    #print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
                    break
            if handin == True:
                gestStart = True
            else:
                gestStart = False
            if (backgroundRemove == True):
                # Remove background - Set pixels further than clipping_distance to grey
                # grey_color = 153
                grey_color = 0
                depth_image_3d = np.dstack(
                    (depth_image, depth_image, depth_image))
                # depth image is 1 channel, color is 3 channels
                bg_removed = np.where((depth_image_3d > clipping_distance) |
                                      (depth_image_3d <= 0), grey_color,
                                      color_image)

                color_image = bg_removed
                draw_image = color_image

            else:
                draw_image = color_image

            # face detection here

            gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)

            if gestCatch == False:
                faces = face_cascade.detectMultiScale(gray, 1.1, 2)
                #print("face: ",len(faces))

                ctr = 0
                idxFace = -1
                minDist = 9999

                if len(faces) > 0:
                    for f in faces:
                        xh, yh, wh, hh = f
                        farea = wh * hh

                        midxf = int(xh + (wh * 0.5))
                        midyf = int(yh + (hh * 0.5))

                        depth_imageS = depth_image * depth_scale
                        deptRef = depth_imageS.item(midyf, midxf)

                        if deptRef <= minDist:
                            idxFace = ctr
                            minDist = deptRef

                        ctr = ctr + 1
                    #print("id face", idxFace)

                    if idxFace >= 0:
                        x, y, w, h = faces[idxFace]
                        cv2.rectangle(draw_image, (x, y), (x + w, y + h),
                                      (255, 0, 0), 2)

            # crop the face then pass to resize

            midx = int(x + (w * 0.5))
            midy = int(y + (h * 0.5))

            xUp = (x - (w * 3))
            yUp = (y - (h * 1.5))

            xDn = ((x + w) + (w * 1))
            yDn = ((y + h) + (h * 2))

            if xUp < 1: xUp = 0
            if xDn >= cameraWidthR: xDn = cameraWidthR

            if yUp < 1: yUp = 0
            if yDn >= cameraHeightR: yDn = cameraHeightR

            if handin == False:
                cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()),
                              (xDn.__int__(), yDn.__int__()), (0, 0, 255), 2)
            else:
                cv2.rectangle(draw_image, (xUp.__int__(), yUp.__int__()),
                              (xDn.__int__(), yDn.__int__()), (0, 255, 0), 2)
            cv2.circle(draw_image, (midx.__int__(), midy.__int__()), 10,
                       (255, 0, 0))

            # region of interest
            roi_gray = gray[yUp.__int__():yDn.__int__(),
                            xUp.__int__():xDn.__int__()]

            #print(cv2.useOptimized())
            '''''


            stateText = "State " + str(currentState)
            cv2.putText(imgTxt, stateText, (10, 200), font, 1, (255, 255, 255), 2,
                        cv2.LINE_AA)

            avtext = "Available Gesture"
            cv2.putText(imgTxt, avtext, (10, 230), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
            for i in range(0, len(availableGest)):
                availGestText = "G" + availableGest[i] + ", "
                cv2.putText(imgTxt, availGestText, (10 + (i * 80), 260), font, 1, (255, 255, 255), 2,
                            cv2.LINE_AA)

            '''
            #            if handin == True and depthFrame==10:
            #                depth_imageS = depth_image * depth_scale
            #                deptRef = depth_imageS.item(midy, midx)
            #                boxColor = color_image.copy()
            #                checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image,handin)
            # find the depth of middle point of face
            if backgroundRemove == True and gestCatch == False:
                #if backgroundRemove == True:
                e1 = cv2.getTickCount()

                depth_imageS = depth_image * depth_scale
                deptRef = depth_imageS.item(midy, midx)
                # print(clipping_distance)

                clipping_distance = (deptRef + 0.2) / depth_scale

                boxColor = color_image.copy()

                #handin = checkhandIn(boxCounter, deptRef, xUp, yUp, xDn, yDn, depth_imageS, draw_image)
                ##handin = checkhandIn2(boxCounter, deptRef, midx, midy, w, h, depth_imageS, boxColor, draw_image)

                #print(handin)

                e2 = cv2.getTickCount()

                times = (e2 - e1) / cv2.getTickFrequency()
                #print(times)

                #handin = False

                ##if handin == True:
                ##    gestStart = True
                ##else:
                ##    gestStart = False

            # print("gest start = ", gestStart)

            if delayBol == False and gestStart == True:
                # your code execution

                if depthFrame < maxFrames:

                    frame = cv2.resize(roi_gray, (img_rows, img_cols))
                    framearray.append(frame)
                    depthFrame = depthFrame + 1
                    txtLoad = txtLoad + "["

                    gestCatch = True

                    # print(depthFrame)

                if depthFrame == maxFrames:
                    dataImg.append(framearray)
                    xx = np.array(dataImg).transpose((0, 2, 3, 1))
                    X = xx.reshape(
                        (xx.shape[0], img_rows, img_cols, maxFrames, channel))
                    X = X.astype('float32')
                    print('X_shape:{}'.format(X.shape))

                    #==================== Update the Weight =======================================
                    newWeightI = []
                    newWeightA = []

                    # find the available gesture in the current state
                    availableGest = getGesture(currentState, ytFsmTable)
                    print(availableGest)
                    availG, ignoreG = translateAvailableGest(
                        availableGest, classGest)
                    print(availG)
                    print(ignoreG)

                    if updatedWeight:
                        weightI = manipWeight(weightDense22I, ignoreG, 1000)

                        newWeightI.append(weightDense2)
                        newWeightI.append(weightI)

                    if updatedWeight2:
                        maxClass = 10
                        weightA = manipWeight(weightDense22A, availG, 1000)

                        newWeightA.append(weightDense2)
                        newWeightA.append(weightA)

                    #=================================================================================

                    if updatedWeight == False and updatedWeight2 == False:
                        newWeightI.append(weightDense2)
                        newWeightI.append(weightDense22A)

                    loaded_model.model.get_layer(
                        name="dense_2").set_weights(newWeightI)
                    if handin == True:
                        # do prediction
                        resc = loaded_model.predict_classes(X)[0]
                        res = loaded_model.predict_proba(X)[0]

                        # find the result of prediction gesture
                        resultC = classGest[resc]
                        nameResultG = nameGest[resc]

                        for a in range(0, len(res)):
                            print("Gesture {}: {} ".format(
                                str(nameGest[a]), str(res[a] * 100)))
                    else:
                        resultC = 0
                        nameResultG = "not enough frame recorded"
                    print(
                        "==============================================================="
                    )

                    if updatedWeight2:
                        loaded_model.model.get_layer(
                            name="dense_2").set_weights(newWeightA)

                        # do prediction
                        resc2 = loaded_model.predict_classes(X)[0]
                        res2 = loaded_model.predict_proba(X)[0]

                        # find the result of prediction gesture
                        resultC2 = classGest[resc2]
                        nameResultG2 = nameGest[resc2]

                        for a in range(0, len(res2)):
                            print("Gesture {}: {} ".format(
                                str(nameGest[a]), str(res2[a] * 100)))

                    # show text of gesture result
                    imgTxt = np.zeros((480, 400, 3), np.uint8)
                    #txt = "Gesture-" + str(resultFsm)
                    if updatedWeight2:
                        if res2[resc2] > res[resc]:
                            txt = "ignored gesture"
                            act = -1
                        else:
                            txt = nameResultG
                            act = resultC
                    else:
                        txt = nameResultG
                        act = resultC

                    print(act)

                    # check with FSM for finding the next state
                    currentState = getNextState(currentState, act, ytFsmTable)
                    print(currentState)

                    framearray = []
                    #dataImg = []
                    txtLoad = ""
                    depthFrame = 0

                    gestCatch = False
                    handin = False
                    delayBol = True

                cv2.putText(imgTxt, txtLoad, (10, 20), font, 0.1,
                            (255, 255, 255), 2, cv2.LINE_AA)
                cv2.putText(imgTxt, txtRecord, (10, 50), font, 1,
                            (255, 255, 255), 2, cv2.LINE_AA)
                cv2.putText(imgTxt, txt, (10, 160), font, 2, (255, 255, 255),
                            2, cv2.LINE_AA)

            # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
            # depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

            # print(delayBol)
            if delayBol == True:
                ctrDelay = ctrDelay + 1
                txtDelay = txtDelay + "["
                txtDel = "Delay"
                cv2.putText(imgTxt, txtDelay, (10, 70), font, 0.1,
                            (255, 255, 255), 2, cv2.LINE_AA)
                cv2.putText(imgTxt, txtDel, (10, 100), font, 1,
                            (255, 255, 255), 2, cv2.LINE_AA)
                if ctrDelay == delayGest:
                    ctrDelay = 0
                    txtDelay = ""
                    delayBol = False

                    gestCatch = False
                    handin = False
                    gestStart = False

            draw_image = cv2.flip(draw_image, 1)
            # Stack both images horizontally
            images = np.hstack((draw_image, imgTxt))

            # Show images
            cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
            cv2.imshow('RealSense', images)
            cv2.waitKey(1)

    finally:

        # Stop streaming
        pipeline.stop()
Ejemplo n.º 26
0
    def __init__(self, width, height, fps, flip, display, record, lab_monitor, specified_devices,
                 snapshot_timer, num_snapshots, snapshot_interval, snapshot_mode, save_params):
        """
        Manage the RealSense output streams
        """
        self.width = width  # Resolution
        self.height = height  # Resolution
        self.fps = fps  # Frame rate
        self.flip = flip  # True to flip cameras vertically when in the living lab
        self.display = display  # True to enable display windows
        self.record = record  # True to enable video writers
        self.lab_monitor = lab_monitor  # Adjust display window sizes for control room or lab monitors (i.e. high / low resolution monitors)
        self.specified_devices = specified_devices  # A list of specified camera serial numbers to be run
        self.snapshot_mode = snapshot_mode  # True to take pictures instead of videos
        self.snapshot_timer = snapshot_timer  # Countdown in seconds till picture taken
        self.num_snapshots = num_snapshots  # How many pictures to take
        self.snapshot_interval = snapshot_interval  # Time in seconds between pictures
        self.save_params = save_params

        assert width > 0, "Invalid width resolution"
        assert height > 0, "Invalid height resolution"
        assert fps > 0, "Invalid frames per second (fps)"
        if self.record:
            assert not self.snapshot_mode, "Should only use record when in video mode (not snapshot mode)"
        if self.snapshot_mode:
            assert not self.record, "Should only use record when in video mode (not snapshot mode)"
            assert self.num_snapshots > 0
            assert self.snapshot_interval >= 0
            assert self.snapshot_timer >= 0

        # Prepare specified configuration (depth & infrared not implemented)
        self.rs_config = rs.config()  # For initialising RealSense
        self.rs_config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.fps)

        # Load the RealSense devices
        self.device_manager = DeviceManager(rs.context(), self.rs_config)
        if len(self.specified_devices) > 0:
            self.device_manager.enable_specified_devices(self.specified_devices)
        else:
            self.device_manager.enable_all_devices()
        assert (len(self.device_manager._available_devices) > 0), \
            "Camera initialisation failed; there are no available devices"

        print(f"Specified devices {self.specified_devices}")
        self.enabled_devices = []
        for (serial, device) in self.device_manager._enabled_devices.items():
            self.enabled_devices.append(serial)
        for device in self.specified_devices:
            assert device in self.enabled_devices, "Specified device is not connected / failed to connect"

        # Set window positions for known cameras based on integer tiling in a 3x3 grid, roughly reflecting their
        # positions in the lab:
        if self.lab_monitor:
            self.default_win_size = (int(1920/3), int(1080/3))  # Good for 1920x1080 display, e.g. monitor in control room
        else:
            self.default_win_size = (int(1920/5), int(1080/5))  # Good for Living Lab TV when using low resolution.

        self.default_win_pos = {
            '2': (2, 2),  # Right, bottom
            '3': (2, 1),  # Middle, bottom
            '4': (2, 0),  # Right, top
            '5': (1, 2),  # Middle, bottom
            '6': (1, 1),  # Middle, middle
            '7': (0, 2),  # Left, bottom
            '8': (0, 0),  # Left, top
        }

        # Convert window positions to pixels
        for key in self.default_win_pos:
            self.default_win_pos[key] = tuple([x * self.default_win_size[i] for i, x in enumerate(self.default_win_pos[key])])

        # This allows simplified window naming and consistent window positioning between runs.
        # If cameras are moved around in the lab, these should be changed.
        self.cam_names = {
            "830112071467": '5',
            "830112071329": '4',
            "831612070394": '2',
            "831612071422": '3',
            "831612071440": '8',
            "831612071526": 'X'
            }

        self.display_windows = {}
        self.video_writers = {}

        # Configure the output directory for recordings
        now = datetime.datetime.now()
        if not os.path.exists(os.path.join(os.getcwd(), "recordings")):
            os.mkdir(os.path.join(os.getcwd(), "recordings"))
        if self.record or self.save_params or self.snapshot_mode:
            self.output_directory = os.path.join(os.getcwd(), "recordings", now.strftime('20%y-%m-%d-%H-%M'))
            if os.path.exists(self.output_directory):
                while os.path.exists(self.output_directory):
                    self.output_directory += "a"
            os.mkdir(self.output_directory)
            assert os.path.exists(self.output_directory)

        # Wait for exposure to balance
        print("Warming up")
        self.warm_up()

        # Write each cameras parameters to json file
        if self.save_params:
            self.save_intrinsics(self.output_directory, self.device_manager.poll_frames())

        print("Loaded lab manager " + str(self.width) + " " + str(self.height) + " " + str(self.fps))

        # Start streaming
        if self.snapshot_mode:
            self.snapshot()
        elif self.display or self.record:
            self.stream()
        else:
            return None
Ejemplo n.º 27
0
if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--save", default=True, action='store_true', help="if save the pos")
    parser.add_argument("--out_dir", default="data/t265_pos", help="dir to save the data")
    opt = parser.parse_args()

    time_str = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(time.time()))
    if not os.path.exists(opt.out_dir) and opt.save:
        os.makedirs(opt.out_dir)
    if opt.save:
        fout = open(os.path.join(opt.out_dir, time_str + ".txt"), "w")

    # init the pipeline
    pipeline = rs.pipeline()
    conf = rs.config()
    conf.enable_stream(rs.stream.pose)
    profile = pipeline.start(conf)

    try:
        while True:
            frames = pipeline.wait_for_frames()
            pose = frames.get_pose_frame()

            if pose:
                data = pose.get_pose_data()
                if opt.save:
                    save_time = time.time()
                    pos = data.translation
                    fout.write("{:.4f}\tx\t{:.4f}\ty\t{:.4f}\tz\t{:.4f}\n".format(save_time, pos.x, pos.y, pos.z))
                if pose.frame_number % 100 == 0:
Ejemplo n.º 28
0
def main(argv):

    size, file_cnt = read_parameters(argv)

    pipeline = rs.pipeline()
    config = rs.config()

    # Get device product line for setting a supporting resolution
    pipeline_wrapper = rs.pipeline_wrapper(pipeline)
    pipeline_profile = config.resolve(pipeline_wrapper)
    device = pipeline_profile.get_device()

    auto_calibrated_device = rs.auto_calibrated_device(device)

    if not auto_calibrated_device:
        print("The connected device does not support auto calibration")
        return

    # config.enable_stream()

    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

    conf = pipeline.start(config)
    calib_dev = rs.auto_calibrated_device(conf.get_device())

    def on_chip_calib_cb(progress):
        print(". ")

    while True:
        try:
            input = raw_input(
                "Please select what the operation you want to do\nc - on chip calibration\nt - tare calibration\ng - get the active calibration\nw - write new calibration\ne - exit\n"
            )

            if input == 'c':
                print("Starting on chip calibration")
                new_calib, health = calib_dev.run_on_chip_calibration(
                    5000, file_cnt, on_chip_calib_cb)
                print("Calibration completed")
                print("health factor = ", health)

            if input == 't':
                print("Starting tare calibration")
                ground_truth = float(
                    raw_input("Please enter ground truth in mm\n"))
                new_calib, health = calib_dev.run_tare_calibration(
                    ground_truth, 5000, file_cnt, on_chip_calib_cb)
                print("Calibration completed")
                print("health factor = ", health)

            if input == 'g':
                calib = calib_dev.get_calibration_table()
                print("Calibration", calib)

            if input == 'w':
                print("Writing the new calibration")
                calib_dev.set_calibration_table(new_calib)
                calib_dev.write_calibration()

            if input == 'e':
                pipeline.stop()
                return

            print("Done\n")
        except Exception as e:
            pipeline.stop()
            print(e)
        except:
            pipeline.stop()
            print("A different Error")
def main():

    # First we set up the cameras to start streaming
    # Define some constants
    resolution_width = 1280  # pixels
    resolution_height = 720  # pixels
    frame_rate = 30  # fps
    dispose_frames_for_stablisation = 30  # frames

    # Enable the streams from all the intel realsense devices
    rs_config = rs.config()
    rs_config.enable_stream(rs.stream.depth, resolution_width,
                            resolution_height, rs.format.z16, frame_rate)
    rs_config.enable_stream(rs.stream.infrared, 1, resolution_width,
                            resolution_height, rs.format.y8, frame_rate)
    rs_config.enable_stream(rs.stream.color, resolution_width,
                            resolution_height, rs.format.bgr8, frame_rate)

    # Use the device manager class to enable the devices and get the frames
    device_manager = DeviceManager(rs.context(), rs_config)
    device_manager.enable_all_devices()
    device_manager._available_devices.sort()
    device_list = device_manager._available_devices

    # Allow some frames for the auto-exposure controller to stablise
    for frame in range(dispose_frames_for_stablisation):
        frames = device_manager.poll_frames_keep()

    assert (len(device_manager._available_devices) > 0)

    #Then we calibrate the images

    # Get the intrinsics of the realsense device
    intrinsics_devices = device_manager.get_device_intrinsics(frames)

    # Set the charuco board parameters for calibration
    aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
    charuco_width = 8
    charuco_height = 5
    square_length = 0.03425
    marker_length = .026

    coordinate_dimentions = 3
    charuco_board = aruco.CharucoBoard_create(charuco_width, charuco_height,
                                              square_length, marker_length,
                                              aruco_dict)
    # Set the charuco board parameters for robot
    aruco_dict_r = aruco.Dictionary_get(aruco.DICT_4X4_250)
    charuco_width_r = 3
    charuco_height_r = 3
    square_length_r = 0.0311
    marker_length_r = .0247

    charuco_board_robot = aruco.CharucoBoard_create(charuco_width_r,
                                                    charuco_height_r,
                                                    square_length_r,
                                                    marker_length_r,
                                                    aruco_dict_r)

    # Set the charuco board parameters for robot
    aruco_dict_ro = aruco.Dictionary_get(aruco.DICT_5X5_100)
    charuco_width_ro = 3
    charuco_height_ro = 3
    square_length_ro = 0.0311
    marker_length_ro = .0247

    charuco_board_robot_2 = aruco.CharucoBoard_create(charuco_width_ro,
                                                      charuco_height_ro,
                                                      square_length_ro,
                                                      marker_length_ro,
                                                      aruco_dict_ro)

    # Decide if you want to use previous calibration or make a new one
    calibration_decision = input(
        'To use previous calibration, press "y". For new calibration press any key \n'
    )
    if calibration_decision != 'y':
        # Choose amount of frames to average
        correct_input = False
        while not correct_input:
            the_input = input(
                "Write amount of frames you want to use to calibrate \n")
            try:
                amount_frames = int(the_input)
                if amount_frames > 0:
                    correct_input = True
                else:
                    print("Frame count has to be positive")
            except ValueError:
                print('Input has to be int')

        # Make a dict to store all images for calibration
        frame_dict = {}
        transform_dict = {}
        rms_dict = {}
        for from_device in device_list:
            transform_dict[from_device] = {}
            rms_dict[from_device] = {}
            for to_device in device_list:
                transform_dict[from_device][to_device] = {}
                rms_dict[from_device][to_device] = np.inf
        print("Starting to take images in 5 seconds")
        time.sleep(5)
        devices_stitched = False

        while not devices_stitched:
            print("taking new set of  images")
            for frame_count in range(amount_frames):
                print("taking image")
                print(amount_frames - frame_count, "images left")
                frames = device_manager.poll_frames_keep()
                time.sleep(0.5)
                frame_dict[frame_count] = {}
                for device in device_list:
                    ir_frame = np.asanyarray(
                        frames[device][(rs.stream.infrared, 1)].get_data())
                    depth_frame = np.asanyarray(
                        post_process_depth_frame(
                            frames[device][rs.stream.depth]).get_data())
                    frame_dict[frame_count][device] = {
                        'ir_frame': ir_frame,
                        'depth_frame': depth_frame
                    }
                del frames

            # Make transfer matrices between all possible cameras
            for idx, from_device in enumerate(device_list[:-1]):
                for to_device in device_list[idx + 1:]:
                    if to_device != from_device:
                        temp_transform, temp_rms = get_transformation_matrix_wout_rsobject(
                            frame_dict, [from_device, to_device],
                            intrinsics_devices, charuco_board)
                        if temp_rms < rms_dict[from_device][to_device]:
                            rms_dict[from_device][to_device] = temp_rms
                            rms_dict[to_device][from_device] = temp_rms
                            transform_dict[from_device][
                                to_device] = temp_transform
                            transform_dict[to_device][
                                from_device] = temp_transform.inverse()

            #Use Djikstra's to fin shortest path and check if all cameras are connected
            transformation_matrices = least_error_transfroms(
                transform_dict, rms_dict)
            if transformation_matrices != 0:
                devices_stitched = True
            # Prints rms error between camera transforms
            test = matrix_viewer(rms_dict)
            print(test)

        # Turns transformation matrices into Transfomation objects
        transformation_devices = {}
        for matrix in transformation_matrices:
            the_matirx = transformation_matrices[matrix]
            transformation_devices[matrix] = Transformation(
                the_matirx[:3, :3], the_matirx[:3, 3])

        # Saves calibration transforms if the user wants to
        save_calibration = input(
            'Press "y" if you want to save calibration \n')
        if save_calibration == "y":
            calibration_name = input
            saved = False
            while not saved:
                name = input(
                    "Type in name of file to save. remember to end name with '.npy' \n"
                )
                try:
                    np.save(name, transformation_matrices)
                    saved = True
                except:
                    print(
                        "could not save, try another name and remember '.npy' in the end"
                    )

        frame_dict.clear()
    else:
        correct_filename = False
        while not correct_filename:
            file_name = input('Type in calibration file name \n')
            try:
                transfer_matirices_save = np.load(file_name, allow_pickle=True)
                transfer_matrices = transfer_matirices_save[()]
                correct_filename = True
            except:
                print('No such file in directory: "', file_name, '"')
        transformation_devices = {}
        for matrix in transfer_matrices:
            the_matrix = transfer_matrices[matrix]
            transformation_devices[matrix] = Transformation(
                the_matrix[:3, :3], the_matrix[:3, 3])

    print("Calibration completed...")

    # Enable the emitter of the devices and extract serial numbers to identify cameras
    device_manager.enable_emitter(True)
    key_list = device_manager.poll_frames().keys()

    while True:
        visualisation = input(
            'Presss "1" for RMS error, "2" for chessboard visualisation and "3" for 3d pointcloud and "4" for robot to camea calibration\n'
        )

        if visualisation == '1':

            calculate_avg_rms_error(device_list, device_manager,
                                    transformation_devices, charuco_board,
                                    intrinsics_devices)

        elif visualisation == '2':

            visualise_chessboard(device_manager, device_list,
                                 intrinsics_devices, charuco_board,
                                 transformation_devices)

        elif visualisation == '3':

            visualise_point_cloud(key_list, resolution_height,
                                  resolution_width, device_manager,
                                  coordinate_dimentions,
                                  transformation_devices)
        elif visualisation == '4':
            robot_calibration = CameraRobotCalibration(transformation_devices,
                                                       device_manager,
                                                       charuco_board_robot_2,
                                                       charuco_board_robot)
            input("Set target in position for calibration\n")
            test = robot_calibration.test_functions()
            print(test)

        else:
            print("Input not recognised")
        return device_extrinsics

    def disable_streams(self):
        self._config.disable_all_streams()


"""
  _____           _    _               
 |_   _|___  ___ | |_ (_) _ __    __ _ 
   | | / _ \/ __|| __|| || '_ \  / _` |
   | ||  __/\__ \| |_ | || | | || (_| |
   |_| \___||___/ \__||_||_| |_| \__, |
									   |___/ 

"""
if __name__ == "__main__":
    try:
        c = rs.config()
        c.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)
        c.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 6)
        c.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 6)
        c.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 6)
        device_manager = DeviceManager(rs.context(), c)
        device_manager.enable_all_devices()
        for k in range(150):
            frames = device_manager.poll_frames()
        device_manager.enable_emitter(True)
        device_extrinsics = device_manager.get_depth_to_color_extrinsics(frames)
    finally:
        device_manager.disable_streams()
Ejemplo n.º 31
0
#!/usr/bin/env python3
"""! This script is a test of the object segmentation code for open CV.
It is primiarily intended as a demo and should not be used without modification
in the final robot setup.
"""

import cv2 as cv
import sys
from os import listdir
import numpy as np
import time
import pyrealsense2 as rs

# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_record_to_file("bagfiles/camera_video")

# Start streaming
pipeline.start(config)

try:
    while True:
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        # depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        if not color_frame:
            continue
Ejemplo n.º 32
0
# -*- coding: utf-8 -*-
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2019 Intel Corporation. All Rights Reserved.

#####################################################
##           librealsense T265 example             ##
#####################################################

# First import the library
import pyrealsense2 as rs

# Declare RealSense pipeline, encapsulating the actual device and sensors
pipe = rs.pipeline()

# Build config object and request pose data
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)

# Start streaming with requested config
pipe.start(cfg)

try:
    for _ in range(50):
        # Wait for the next set of frames from the camera
        frames = pipe.wait_for_frames()

        # Fetch pose frame
        pose = frames.get_pose_frame()
        if pose:
            # Print some of the pose data to the terminal
            data = pose.get_pose_data()
Ejemplo n.º 33
0
  def getParameter(self):
    # Configure depth and color streams
    pipeline = rs.pipeline()
    config = rs.config()

    if self.EnableColor is True:
        config.enable_stream(rs.stream.color, int(self.width), int(self.height), rs.format.bgr8, 30)

    if self.EnableInfrared is True:
        config.enable_stream(rs.stream.infrared, 1, int(self.width), int(self.height), rs.format.y8, 30)
        config.enable_stream(rs.stream.infrared, 2, int(self.width), int(self.height), rs.format.y8, 30)
        
    if self.EnableDepth is True:
        config.enable_stream(rs.stream.depth, int(self.width), int(self.height), rs.format.z16, 30)
        
    # Start streaming
    cfg = pipeline.start(config)

    # Get Video Stream Intrinsics
    if self.printPara is True:
      print("=======================================================")
      print("Get Video Stream Intrinsics")
      print("=======================================================")
      
    if self.EnableColor is True:
      colorStream = cfg.get_stream(rs.stream.color)
      colorProfile = colorStream.as_video_stream_profile()
      self.colorIntrin = colorProfile.get_intrinsics()
      if self.printPara is True:
        print("Color Intrinsics:")
        print(self.colorIntrin)
        print("")

    if self.EnableInfrared is True:
      infraredStream = cfg.get_stream(rs.stream.infrared, 1)
      infraredProfile = infraredStream.as_video_stream_profile()
      self.infraredIntrin = infraredProfile.get_intrinsics()
      if self.printPara is True:
        print("Infrared Intrinsics:")
        print(self.infraredIntrin)
        print("")

    if self.EnableDepth is True:
      depthStream = cfg.get_stream(rs.stream.depth)
      depthProfile = depthStream.as_video_stream_profile()
      self.depthIntrin = depthProfile.get_intrinsics()
      if self.printPara is True:
        print("Depth Intrinsics:")
        print(self.depthIntrin)
        print("")

      # Get and Apply Depth-to-Stream Extrinsics
        print("\n=======================================================")
        print("Get and Apply Depth-to-Stream Extrinsics")
        print("=======================================================")
        print("Color Extrinsics:")
      
      colorExtrin = depthStream.get_extrinsics_to(colorStream)
      if self.printPara is True:
        print(colorExtrin)
        print("")
        print("Infrared Extrinsics:")
      
      infraredExtrin = depthStream.get_extrinsics_to(infraredStream)
      if self.printPara is True:
        print(infraredExtrin)

    if self.EnableInfrared is True:
      # Get Disparity Baseline
      if self.printPara is True:
        print("\n=======================================================")
        print("Get Disparity Baseline")
        print("=======================================================")
      
      infraredStream2 = cfg.get_stream(rs.stream.infrared, 2)
      self.baseline = infraredStream2.get_extrinsics_to(infraredStream)
      if self.printPara is True:
        print(self.baseline)
    
    pipeline.stop()
Ejemplo n.º 34
0
def main():
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    server.bind(('127.0.0.1', 8000))
    server.listen(5)
    print("waiting msg ...")
    conn, clint_add = server.accept()

    args = parse_args()
    torch.backends.cudnn.enabled = True
    torch.backends.cudnn.benchmark = True

    load_config(cfg, args.config)
    logger = Logger(-1, use_tensorboard=False)
    predictor = Predictor(cfg, args.model, logger, device='cuda:0')
    logger.log('Press "Esc", "q" or "Q" to exit.')
    if args.demo == 'image':
        if os.path.isdir(args.path):
            files = get_image_list(args.path)
        else:
            files = [args.path]
        files.sort()
        for image_name in files:
            meta, res = predictor.inference(image_name)
            predictor.visualize(res, meta, cfg.class_names, 0.35)
            ch = cv2.waitKey(0)
            if ch == 27 or ch == ord('q') or ch == ord('Q'):
                break
    elif args.demo == 'video' or args.demo == 'webcam':
        pipeline = rs.pipeline()
        # 创建 config 对象:
        config = rs.config()
        config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        # Start streaming
        pipeline.start(config)
        #cap = cv2.VideoCapture(args.path if args.demo == 'video' else args.camid)
        while True:
            next_frames = pipeline.wait_for_frames()
            get_next_color_frame = next_frames.get_color_frame()
            frame = np.asanyarray(get_next_color_frame.get_data())
            meta, res = predictor.inference(frame)

            predictor.visualize(res, meta, cfg.class_names, 0.70)
            all_box = []
            for label in res:
                for bbox in res[label]:
                    score = bbox[-1]
                    if score > 0.70:
                        x0, y0, x1, y1 = [int(i) for i in bbox[:4]]
                        all_box.append([label, x0, y0, x1, y1, score])
            all_box.sort(key=lambda v: v[5])

            send_data_byte = bytes(0)
            time.sleep(0.005)
            if len(all_box) == 0:
                leftup_rightdown_corner = [-1, 0, 0, 0, 0, time.time(), 'b']
                for i in range(len(leftup_rightdown_corner)):
                    #print(pickup_leftup_rightdown_corner[i])
                    pickup_senddata = str(leftup_rightdown_corner[i]) + ','
                    # print(pickup_senddata.encode())
                    send_data_byte += pickup_senddata.encode()
                    # print(send_data_byte)
                conn.send(send_data_byte)
            else:
                zzw = all_box[-1]
                label, x0, y0, x1, y1, score = zzw
                leftup_rightdown_corner = [1, x0, y0, x1, y1, time.time(), 'a']
                for i in range(len(leftup_rightdown_corner)):
                    target_senddata = str(leftup_rightdown_corner[i]) + ','
                    send_data_byte += target_senddata.encode()
                conn.send(send_data_byte)

            ch = cv2.waitKey(1)
            if ch == 27 or ch == ord('q') or ch == ord('Q'):
                break
Ejemplo n.º 35
0
def main():
    #out = cv2.VideoWriter('./data/output.avi', -1, 20.0,(1280, 720))
    pipeline = rs.pipeline()
    config = rs.config()

    vis = o3d.visualization.Visualizer()
    vis.create_window('PCD', width=1280, height=720)
    pointcloud = o3d.geometry.PointCloud()
    vis.add_geometry(pointcloud)
    geom_added = False

    # note: using 640 x 480 depth resolution produces smooth depth boundaries
    #       using rs.format.bgr8 for color image format for OpenCV based image visualization
    config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30)

    config.enable_device_from_file("./data/realsense.bag",
                                   repeat_playback=True)

    # Start streaming
    profile = pipeline.start(config)
    depth_sensor = profile.get_device().first_depth_sensor()

    # Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_scale = depth_sensor.get_depth_scale()

    # We will not display the background of objects more than
    #  clipping_distance_in_meters meters away
    clipping_distance_in_meters = 3  # 3 meter
    clipping_distance = clipping_distance_in_meters / depth_scale

    # Create an align object
    # rs.align allows us to perform alignment of depth frames to others frames
    # The "align_to" is the stream type to which we plan to align depth frames.
    align_to = rs.stream.color
    align = rs.align(align_to)

    # Streaming loop
    frame_count = 0
    intrinsics = profile.get_stream(
        rs.stream.depth).as_video_stream_profile().get_intrinsics()
    pcd_old = o3d.geometry.PointCloud()

    while True:
        # Get frameset of color and depth
        frames = pipeline.wait_for_frames()

        # Align the depth frame to color frame
        aligned_frames = align.process(frames)

        # Get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame()
        color_frame = aligned_frames.get_color_frame()

        # Validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            continue

        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

        img_depth = o3d.geometry.Image(depth_image)
        img_color = o3d.geometry.Image(color_image)
        rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
            img_color,
            img_depth,
            depth_trunc=clipping_distance_in_meters,
            convert_rgb_to_intensity=False)

        pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
            intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy,
            intrinsics.ppx, intrinsics.ppy)
        pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
            rgbd, pinhole_camera_intrinsic)
        # o3d.visualization.draw_geometries([pcd])
        #print("Update")
        start = time.time()
        plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                                 ransac_n=3,
                                                 num_iterations=60)
        [a, b, c, d] = plane_model
        print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

        outlier_cloud = pcd.select_by_index(inliers, invert=True)
        end = time.time()
        print("Plane segmentation took: " + str((end - start) * 1000) + "ms")
        # plane_model, inliers = outlier_cloud.segment_plane(distance_threshold=0.01,
        #                                          ransac_n=3,
        #                                          num_iterations=1000)
        # [a, b, c, d] = plane_model
        # print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
        #
        # inlier_cloud = outlier_cloud.select_by_index(inliers)
        # inlier_cloud.paint_uniform_color([1.0, 0, 0])
        # outlier_cloud = outlier_cloud.select_by_index(inliers, invert=True)
        pcd = outlier_cloud.voxel_down_sample(voxel_size=0.007)
        pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0],
                       [0, 0, 0, 1]])
        print(pcd)

        start = time.time()
        labels = np.array(
            pcd.cluster_dbscan(eps=0.015, min_points=20, print_progress=False))
        # with o3d.utility.VerbosityContextManager(
        #         o3d.utility.VerbosityLevel.Debug) as cm:
        #     labels = np.array(
        #         pcd.cluster_dbscan(eps=0.02, min_points=15, print_progress=False))
        end = time.time()
        print("Clustering took: " + str((end - start) * 1000) + "ms")
        max_label = labels.max()
        objects = []
        #todo paralelize
        for i in range(0, max_label + 1):
            objects.append(TrackingObject(i))

        point_array = np.asarray(pcd.points)
        for i, point in enumerate(point_array):
            cluster_id = labels[i]
            #print(cluster_id)
            if cluster_id != -1:
                objects[cluster_id].points.append(point)

        for obj in objects:
            obj.calc_bounding_box(intrinsics)

        print(f"point cloud has {max_label + 1} clusters")

        colors = plt.get_cmap("tab20")(labels /
                                       (max_label if max_label > 0 else 1))
        colors[labels < 0] = 0
        pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
        #o3d.visualization.draw_geometries([pcd])
        for obj in objects:
            cv2.rectangle(color_image, obj.bounding_box[0],
                          obj.bounding_box[1], (0, 255, 0), 2)
        cv2.imshow('bgr', color_image)
        key = cv2.waitKey(1)
        if key == ord('q'):
            break

        pointcloud.points = pcd.points
        pointcloud.colors = pcd.colors

        if geom_added == False:
            vis.add_geometry(pointcloud)
            geom_added = True

        #vis.capture_screen_image("./data/test.png")
        #screenshot = np.asanyarray(buf)
        #cv.ims
        #out.write(screenshot)

        vis.update_geometry(pointcloud)
        vis.poll_events()
        vis.update_renderer()
        frame_count += 1

    vis.destroy_window()
    del vis
    #out.release()
    pipeline.stop()
Ejemplo n.º 36
0
def gatherCameraImage(gesture, iterations):
    # Create a pipeline
    pipeline = rs.pipeline()

    #Create a config and configure the pipeline to stream
    #  different resolutions of color and depth streams
    config = rs.config()
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

    # Start streaming
    profile = pipeline.start(config)

    # Store images to process later so capturing is fast
    storedImages = {}

    print("collecting images")
    for iteration in range(iterations):
        print("collected image : " + str(iteration))

        if (iteration != 0 and iteration % 10 == 0):
            pipeline.stop()
            profile = pipeline.start(config)

        # Getting the depth sensor's depth scale (see rs-align example for explanation)
        depth_sensor = profile.get_device().first_depth_sensor()
        depth_scale = depth_sensor.get_depth_scale()

        # We will be removing the background of objects more than
        #  clipping_distance_in_meters meters away
        clipping_distance_in_meters = 1  #1 meter
        clipping_distance = clipping_distance_in_meters / depth_scale

        # Create an align object
        # rs.align allows us to perform alignment of depth frames to others frames
        # The "align_to" is the stream type to which we plan to align depth frames.
        align_to = rs.stream.color
        align = rs.align(align_to)

        # Get frameset of color and depth
        frames = pipeline.wait_for_frames()
        # frames.get_depth_frame() is a 640x360 depth image

        # Align the depth frame to color frame
        aligned_frames = align.process(frames)

        # Get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame(
        )  # aligned_depth_frame is a 640x480 depth image
        # color_frame = aligned_frames.get_color_frame()

        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        # color_image = np.asanyarray(color_frame.get_data())

        #Storing images so we can process later
        key = str(len(storedImages))
        storedImages[key] = depth_image
        # Remove background - Set pixels further than clipping_distance to grey
        grey_color = 153
        depth_image_3d = np.dstack(
            (depth_image, depth_image,
             depth_image))  #depth image is 1 channel, color is 3 channels

        # Render images
        depth_colormap = cv2.applyColorMap(
            cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
        images = np.hstack((depth_colormap, depth_colormap))
        cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE)
        cv2.imshow('Align Example', images)
        key = cv2.waitKey(1)

    pipeline.stop()
    processedImages = []

    print("processing images")
    #Processing each stored image
    startIdx = startIndex(gesture)

    for i in range(0, iterations):
        print("processing image : " + str(i))
        thread = dcThread(storedImages[str(i)], startIdx + i, gesture)
        thread.start()
Ejemplo n.º 37
0
def main():
    # start node
    rospy.init_node("kinect_find_xyz")
    rospy.loginfo("kinect_find_xyz node initialized")

    # create Point32 publisher
    pointPub = rospy.Publisher(pointPubTopic, PointStamped, queue_size=1)

    # create moving average filters
    WINDOW_SIZE = 4
    moving_avg_x = MovingWindowAverage(WINDOW_SIZE)
    moving_avg_y = MovingWindowAverage(WINDOW_SIZE)
    moving_avg_z = MovingWindowAverage(WINDOW_SIZE)

    # Create a pipeline
    pipeline = rs.pipeline()

    # Create a config and configure the pipeline to stream
    # different resolutions of color and depth streams
    config = rs.config()

    # Get device product line for setting a supporting resolution
    pipeline_wrapper = rs.pipeline_wrapper(pipeline)
    pipeline_profile = config.resolve(pipeline_wrapper)
    device = pipeline_profile.get_device()
    device_product_line = str(device.get_info(rs.camera_info.product_line))

    # enable depth and color streams
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

    # Start streaming
    profile = pipeline.start(config)

    # Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()

    # Create an align object
    # rs.align allows us to perform alignment of depth frames to others frames
    # The "align_to" is the stream type to which we plan to align depth frames.
    align_to = rs.stream.color
    align = rs.align(align_to)

    while (not rospy.is_shutdown()):
        # grab frames from intel RealSense
        frames = pipeline.wait_for_frames()

        # Align the depth frame to color frame
        aligned_frames = align.process(frames)

        # Get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame(
        )  # aligned_depth_frame is a 640x480 depth image
        color_frame = aligned_frames.get_color_frame()

        # get depth frame intrinsics
        depth_intrin = aligned_depth_frame.profile.as_video_stream_profile(
        ).intrinsics

        # Validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            continue

        # convert to numpy arrays
        numpyRGB = np.asanyarray(color_frame.get_data())
        numpyDepth = np.asanyarray(aligned_depth_frame.get_data())

        # find min enclosing circle
        status, circle = findMinEnclosingCircle(numpyRGB)

        if (status):
            # draw circles if they are found
            (x, y), radius = circle
            cv2.circle(numpyRGB, (int(x), int(y)), int(radius), (0, 255, 0), 2)

            # find depth of detected circle
            reading = circle_to_realworld(aligned_depth_frame, circle,
                                          depth_intrin)

            average_x = moving_avg_x.addAndProc(reading[0])
            average_y = moving_avg_y.addAndProc(reading[1])
            average_z = moving_avg_z.addAndProc(reading[2])

            # invert sign of y-point to align with standard cartesian
            # views
            average_y = -1 * average_y

            # create PointStamped message and publish
            pointMessage = PointStamped()
            pointMessage.header.stamp = rospy.Time.now()
            pointMessage.point.x = average_x
            pointMessage.point.y = average_y
            pointMessage.point.z = average_z
            pointPub.publish(pointMessage)

        # publish color image
        imgMsg = imgBridge.cv2_to_imgmsg(numpyRGB, "bgr8")
        imgPub.publish(imgMsg)

        # display images
        depth_colormap = cv2.applyColorMap(
            cv2.convertScaleAbs(numpyDepth, alpha=0.03), cv2.COLORMAP_JET)
        cv2.imshow('Depth', depth_colormap)
        cv2.imshow('Video', numpyRGB)
        if (cv2.waitKey(1) & 0xFF == ord('q')):
            break
Ejemplo n.º 38
0
def run_loop(bag_path, seg_model, seg_opts, save_images=False, output_mode=0):
    if save_images:
        create_folders()
    # Create pipeline
    pipeline = rs.pipeline()
    # Create a config object
    config = rs.config()
    # Tell config that we will use a recorded device from filem to be used by the pipeline through playback.
    rs.config.enable_device_from_file(config, args.input)
    # Start streaming from file
    Pipe = pipeline.start(config)

    # Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_sensor = Pipe.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()
    print("Depth Scale is: ", depth_scale)  # can be commented out

    if output_mode == 0 or output_mode == 1:
        # Create opencv window to render image in
        cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL)

    flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]

    # Create colorizer object
    colorizer = rs.colorizer()
    idx = 0
    # initial frame delay
    idx_limit = 30

    # pre_seg_mask_sum = None  # previous frame path segmentation area - isn't being used right now

    # Streaming loop
    try:
        while True:
            idx += 1
            # Get frameset of depth
            frames = pipeline.wait_for_frames()
            # ignore first idx frames
            if idx < idx_limit:
                continue
            else:
                pass

            align = rs.align(rs.stream.color)
            frames = align.process(frames)

            # Get color frame
            color_frame = frames.get_color_frame()
            # Get intrinsic in Open3d format for mode 2 and 3 for point cloud output
            if output_mode == 1 or output_mode == 2:
                intrinsic = o3d.camera.PinholeCameraIntrinsic(
                    get_intrinsic_matrix(color_frame))
            # Get depth frame
            depth_frame = frames.get_depth_frame()
            # Print intrinsics and extrinsics - not necessary : can be commented out
            if idx == idx_limit:
                camera_intrinsics(color_frame, depth_frame, Pipe)

            color_image = np.asanyarray(color_frame.get_data())

            ### Add SEGMENTATION part here ###
            pred = test(color_image, seg_model, seg_opts)

            # pavement, floor, road, earth/ground, field, path, dirt/track - chosen classes for the model selected (we'd like an oversegmentation of the path)
            seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
                pred == 13) | (pred == 29) | (pred == 52) | (
                    pred == 91)  #.astype(np.uint8)

            if idx == idx_limit:  # 1st frame detection needs to be robust
                pre_seg_mask_sum = np.sum(seg_mask)
            # checking for bad detection
            new_seg_sum = np.sum(seg_mask)
            diff = abs(new_seg_sum - pre_seg_mask_sum)
            # if diff > pre_seg_mask_sum/15:  # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
            #     seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
            #     del new_seg_sum
            # else:
            pre_seg_mask_sum = new_seg_sum
            ### mask Hole filling
            seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
            seg_mask = seg_mask.astype(np.uint8)
            #####
            seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))

            pred_color = colorEncode(
                pred,
                loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
            ##################################

            depth_frame = depth_filter(depth_frame)
            depth_array = np.asarray(depth_frame.get_data())
            # Colorize depth frame to jet colormap
            depth_color_frame = colorizer.colorize(depth_frame)
            # Convert depth_frame to numpy array to render image in opencv
            depth_color_image = np.asanyarray(depth_color_frame.get_data())

            ############ Plane Detection
            ## need to add smoothening between frames - by plane weights' variance?
            try:
                ### need to add multithreading here (and maybe other methods?)
                planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
                    loop=5)
            except TypeError as e:
                try:
                    print("plane mask 1st error")
                    planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
                        img_color=color_image * seg_mask_3d,
                        img_depth=depth_array * seg_mask)
                except TypeError as e:
                    print("plane mask not detected-skipping frame")
                    continue
                    ## removed this part
                    planes_mask = np.ones_like(depth_array).astype(np.uint8)
                    planes_mask = np.dstack(
                        (planes_mask, planes_mask, planes_mask))
            ##############################################
            ## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
            planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
            planes_mask_binary = planes_mask_binary.astype(np.uint8)
            # Clean plane mask object detection by seg_mask
            planes_mask_binary *= seg_mask
            planes_mask_binary_3d = np.dstack(
                (planes_mask_binary, planes_mask_binary, planes_mask_binary))

            edges = planes_mask_binary - nd.morphology.binary_dilation(
                planes_mask_binary
            )  # edges calculation - between travesable and non-traversable path
            #############################################

            if output_mode == 1 or output_mode == 2:
                odepth_image = o3d.geometry.Image(depth_array * edges)
                ocolor_image = o3d.geometry.Image(color_image * np.dstack(
                    (edges, edges, edges)))

                rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
                    ocolor_image,
                    odepth_image,
                    depth_scale=1.0 / depth_scale,
                    depth_trunc=10,  # set to 10 metres
                    convert_rgb_to_intensity=False)
                temp = o3d.geometry.PointCloud.create_from_rgbd_image(
                    rgbd_image, intrinsic)
                temp.transform(flip_transform)
                # temp = temp.voxel_down_sample(0.03)

            # Point cloud output of frame is appended to the list
            if output_mode == 1 or output_mode == 2:
                output_list.append(temp)

            # image format conversion for cv2 visualization/output
            if output_mode == 0 or output_mode == 1 or save_images == True:
                pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR)
                color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
                edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR)
                ## for displaying seg_mask
                seg_mask = (np.array(seg_mask) * 255).astype(np.uint8)
                seg_mask = cv2.cvtColor(
                    seg_mask, cv2.COLOR_GRAY2BGR)  # segmentation binary mask
                final_output_mask = cv2.cvtColor(
                    planes_mask_binary,
                    cv2.COLOR_GRAY2BGR)  # final traversable path mask

            if output_mode == 0 or output_mode == 1:
                # # Blending rgb and depth images for display - can check alignment with this as well
                alpha = 0.2
                beta = (1.0 - alpha)
                dst = cv2.addWeighted(
                    color_image, alpha, pred_color, beta, 0.0
                )  # color and segmentation output from ADE20K model blended
                dst2 = cv2.addWeighted(depth_color_image, alpha, color_image,
                                       beta,
                                       0.0)  # color and depth blended together
                ##################################

                ### delete later if needed - color image masked by final traversable path
                final_output = color_image * planes_mask_binary_3d
                mask = (final_output[:, :, 0] == 0) & (
                    final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0)
                final_output[:, :, :3][mask] = [255, 255, 255]
                ######

                ### Select outputs for visualization - we've chosen some as default
                image_set1 = np.vstack((dst, dst2))
                # image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask))
                # image_set2 = np.vstack((dst, final_output))
                image_set2 = np.vstack((edges, final_output))
                ### Choose which images you want to display from above
                combined_images = np.concatenate((image_set1, image_set2),
                                                 axis=1)

            if save_images == True:
                # Outputs saved - you can modify this
                cv2.imwrite("output/visualization/frame%d.png" % idx,
                            combined_images)
                cv2.imwrite("data/color/frame%d.png" % idx,
                            color_image)  # save frame as JPEG file
                cv2.imwrite("data/depth/frame%d.png" % idx,
                            depth_array)  # save frame as JPEG file
                cv2.imwrite("output/edges/frame%d.png" % idx,
                            edges)  # save frame as JPEG file
                cv2.imwrite("output/segmentation_mask/frame%d.png" % idx,
                            seg_mask)  # save frame as JPEG file
                cv2.imwrite("output/output_mask/frame%d.png" % idx,
                            final_output_mask)  # save frame as JPEG file

            if output_mode == 0 or output_mode == 1:
                try:
                    cv2.imshow('Full Stream', combined_images)
                except TypeError as e:
                    print(idx, e)
                key = cv2.waitKey(1)
                # if pressed escape exit program
                if key == 27:
                    cv2.destroyAllWindows()
                    break
    finally:
        pipeline.stop()
        if output_mode == 0 or output_mode == 1:
            cv2.destroyAllWindows()
    return
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.

###############################################
##      Open CV and Numpy integration        ##
###############################################

import pyrealsense2 as rs
import numpy as np
import cv2

# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start streaming
pipeline.start(config)

try:
    while True:

        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        if not depth_frame or not color_frame:
            continue

        # Convert images to numpy arrays
Ejemplo n.º 40
0
def create_pipeline(config: dict):
    """Sets up the pipeline to extract depth and rgb frames

    Arguments:
        config {dict} -- A dictionary mapping for configuration. see default.yaml

    Returns:
        tuple -- pipeline, process modules, filters, t265 device (optional)
    """
    # Create pipeline and config for D4XX,L5XX
    pipeline = rs.pipeline()
    rs_config = rs.config()

    # IF t265 is enabled, need to handle seperately
    t265_dev = None
    t265_sensor = None
    t265_pipeline = rs.pipeline()
    t265_config = rs.config()

    if config['playback']['enabled']:
        # Load recorded bag file
        rs.config.enable_device_from_file(
            rs_config, config['playback']['file'], config['playback'].get('repeat', False))

        # This code is only activated if the user points to a T265 Recorded Bag File
        if config['tracking']['enabled']:
            rs.config.enable_device_from_file(
                t265_config, config['tracking']['playback']['file'], config['playback'].get('repeat', False))

            t265_config.enable_stream(rs.stream.pose)
            t265_pipeline.start(t265_config)
            profile_temp = t265_pipeline.get_active_profile()
            t265_dev = profile_temp.get_device()
            t265_playback = t265_dev.as_playback()
            t265_playback.set_real_time(False)

    else:
        # Ensure device is connected
        ctx = rs.context()
        devices = ctx.query_devices()
        if len(devices) == 0:
            logging.error("No connected Intel Realsense Device!")
            sys.exit(1)

        if config['advanced']:
            logging.info("Attempting to enter advanced mode and upload JSON settings file")
            load_setting_file(ctx, devices, config['advanced'])

        if config['tracking']['enabled']:
            # Cycle through connected devices and print them
            for dev in devices:
                dev_name = dev.get_info(rs.camera_info.name)
                print("Found {}".format(dev_name))
                if "Intel RealSense D4" in dev_name:
                    pass
                elif "Intel RealSense T265" in dev_name:
                    t265_dev = dev
                elif "Intel RealSense L515" in dev_name:
                    pass

            if config['tracking']['enabled']:
                if len(devices) != 2:
                    logging.error("Need 2 connected Intel Realsense Devices!")
                    sys.exit(1)
                if t265_dev is None:
                    logging.error("Need Intel Realsense T265 Device!")
                    sys.exit(1)

                if t265_dev:
                    # Unable to open as a pipeline, must use sensors
                    t265_sensor = t265_dev.query_sensors()[0]
                    profiles = t265_sensor.get_stream_profiles()
                    pose_profile = [profile for profile in profiles if profile.stream_name() == 'Pose'][0]
                    t265_sensor.open(pose_profile)
                    t265_sensor.start(callback_pose)
                    logging.info("Started streaming Pose")

    rs_config.enable_stream(
        rs.stream.depth, config['depth']['width'],
        config['depth']['height'],
        rs.format.z16, config['depth']['framerate'])
    # other_stream, other_format = rs.stream.infrared, rs.format.y8
    rs_config.enable_stream(
        rs.stream.color, config['color']['width'],
        config['color']['height'],
        rs.format.rgb8, config['color']['framerate'])

    # Start streaming
    pipeline.start(rs_config)
    profile = pipeline.get_active_profile()

    depth_sensor = profile.get_device().first_depth_sensor()
    color_sensor = profile.get_device().first_color_sensor()

    depth_scale = depth_sensor.get_depth_scale()
    # depth_sensor.set_option(rs.option.global_time_enabled, 1.0)
    # color_sensor.set_option(rs.option.global_time_enabled, 1.0)

    if config['playback']['enabled']:
        dev = profile.get_device()
        playback = dev.as_playback()
        playback.set_real_time(False)

    # Processing blocks
    filters = []
    decimate = None
    align = rs.align(rs.stream.color)
    depth_to_disparity = rs.disparity_transform(True)
    disparity_to_depth = rs.disparity_transform(False)
    # Decimation
    if config.get("filters").get("decimation"):
        filt = config.get("filters").get("decimation")
        if filt.get('active', True):
            filt.pop('active', None)  # Remove active key before passing params
            decimate = rs.decimation_filter(**filt)

    # Spatial
    if config.get("filters").get("spatial"):
        filt = config.get("filters").get("spatial")
        if filt.get('active', True):
            filt.pop('active', None)  # Remove active key before passing params
            my_filter = rs.spatial_filter(**filt)
            filters.append(my_filter)

    # Temporal
    if config.get("filters").get("temporal"):
        filt = config.get("filters").get("temporal")
        if filt.get('active', True):
            filt.pop('active', None)  # Remove active key before passing params
            my_filter = rs.temporal_filter(**filt)
            filters.append(my_filter)

    process_modules = (align, depth_to_disparity, disparity_to_depth, decimate)

    intrinsics = get_intrinsics(pipeline, rs.stream.color)
    proj_mat = create_projection_matrix(intrinsics)

    sensor_meta = dict(depth_scale=depth_scale)
    config['sensor_meta'] = sensor_meta
    
    # Note that sensor must be saved so that it is not garbage collected
    t265_device = dict(pipeline=t265_pipeline, sensor=t265_sensor)

    return pipeline, process_modules, filters, proj_mat, t265_device