def main(): video = VideoCapture(video_sources.video_6) work_area = WorkAreaView(video_sources.video_6_work_area_markers) vc = VideoController(10, 'pause') video_wnd, = Wnd.create('video') # h_wnd, s_wnd, v_wnd = Wnd.create('H', 'S', 'V') # L_wnd, a_wnd, b_wnd = Wnd.create('L', 'a', 'b') frames_iter = work_area.skip_non_area(video.frames()) for frame, _ in frames_iter: # hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # h_wnd.imshow(hsv[:, :, 0]) # s_wnd.imshow(hsv[:, :, 1]) # v_wnd.imshow(hsv[:, :, 2]) # lab = cv2.cvtColor(frame, cv2.COLOR_BGR2Lab) # L_wnd.imshow(lab[:, :, 0]) # a_wnd.imshow(lab[:, :, 1]) # b_wnd.imshow(lab[:, :, 2]) vis_img = utils.put_frame_pos(frame, video.frame_pos(), xy=(2, 55)) video_wnd.imshow(vis_img) if vc.wait_key() == 27: break video.release()
def main(): video = VideoCapture(video_sources.video_2) frame = video.read() backSubtractor = BackgroundSubtractorAVG(0.2, denoise(frame)) for frame in video.frames(): with utils.timeit_context(): frame = denoise(frame) foreGround = backSubtractor.getForeground(frame) # Apply thresholding on the background and display the resulting mask ret, mask = cv2.threshold(foreGround, 15, 255, cv2.THRESH_BINARY) cv2.imshow('input', frame) cv2.imshow('foreground', foreGround) # Note: The mask is displayed as a RGB image, you can # display a grayscale image by converting 'foreGround' to # a grayscale before applying the threshold. cv2.imshow('mask', mask) if cv2.waitKey(10) & 0xFF == 27: break video.release() cv2.destroyAllWindows()
def main(): video = VideoCapture(video_sources.video_2) workArea = WorkAreaView(video_sources.video_2_work_area_markers) vc = VideoController(10, 'pause') (video_wnd, bin_diff_wnd, gray_diff_wnd, colorDiffWnd, learned_BG_wnd) = Wnd.create('video', 'binary diff', 'gray diff', 'color diff', 'Learned BG') colorAbsDiffWnd = Wnd('color_abs_diff') segmentedWnd = Wnd('segmented') segmenter = Segmenter() frames_iter = workArea.skip_non_area(video.frames()) motionDetector = MotionDetector(next(frames_iter)[0], 3) backgroundModel = BackgroundModel(15) prevBackground = None for frame, _ in frames_iter: motionDetector.detect(frame) if motionDetector.motionEnded(): # calc fgMask mask, gray_diff, color_diff, colorAbsDiff = calcForegroundMask( prevBackground, frame) # bin_diff_wnd.imshow(resize(mask, 0.5)) bin_diff_wnd.imshow(cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)) # gray_diff_wnd.imshow(resize(gray_diff, .5)) # colorDiffWnd.imshow(resize(color_diff, .5)) # colorAbsDiffWnd.imshow(resize(colorAbsDiff, .5)) markers, objectsCount = segmenter.segment(mask) segmentedWnd.imshow( resize(Segmenter.markersToDisplayImage(markers, objectsCount), .5)) backgroundModel = BackgroundModel(15) if motionDetector.isSilence(): backgroundModel.learn(frame, foregroundMask=None) learned_BG_wnd.imshow(resize(backgroundModel.learned, .5)) if motionDetector.motionStarted(): prevBackground = backgroundModel.learned backgroundModel = None learned_BG_wnd.imshow(resize(prevBackground, .5)) # VIS vis_img = motionDetector.indicateCurrentState(frame.copy()) vis_img = utils.put_frame_pos(vis_img, video.frame_pos(), xy=(2, 55)) video_wnd.imshow(vis_img) # bin_diff_wnd.imshow(resize(motionDetector.bin_diff, .5)) # gray_diff_wnd.imshow(resize(motionDetector.gray_diff, .5)) # VIS END if vc.wait_key() == 27: break video.release()
def main(): video = VideoCapture(video_sources.video_6) work_area = WorkAreaView(video_sources.video_6_work_area_markers) vc = VideoController(10, 'pause') (video_wnd, bin_diff_wnd, gray_diff_wnd, frame0_diff_wnd, learned_BG_wnd) = Wnd.create('video', 'binary diff', 'gray diff', 'diff with frame0', 'Learned BG') frames_iter = work_area.skip_non_area(video.frames()) motion_detector = MotionDetector(next(frames_iter)[0], 3) background = BackgroundModel(motion_detector, 15) for frame, _ in frames_iter: motion_detector.detect(frame) if not background.done: background.learn() else: if motion_detector.motion_ended(): frame0_diff = cv2.absdiff(background.learned, frame) gray_of_color_diff = Helper.to_gray(frame0_diff) frame0_diff_wnd.imshow( resize( np.hstack( (frame0_diff, Helper.to_bgr(gray_of_color_diff))), .5)) _, binary = cv2.threshold(gray_of_color_diff, 35, 255, cv2.THRESH_BINARY) cv2.imshow('1 binary', resize(binary, .5)) # VIS if background.done: learned_BG_wnd.imshow(resize(background.learned, 1)) vis_img = motion_detector.put_current_state(frame.copy()) vis_img = utils.put_frame_pos(vis_img, video.frame_pos(), xy=(2, 55)) video_wnd.imshow(vis_img) # bin_diff_wnd.imshow(resize(motion_detector.bin_diff, .5)) # gray_diff_wnd.imshow(resize(motion_detector.gray_diff, .5)) # VIS END if vc.wait_key() == 27: break video.release()
def main(): feature_params = dict(maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7) lk_params = dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) video = VideoCapture(video_sources.video_2) wnd = CvNamedWindow('video') vc = VideoController(delay=50) prev_gray = None po = None tracking = False for frame in video.frames(): if tracking: frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # calculate optical flow p1, st, err = cv2.calcOpticalFlowPyrLK(prev_gray, frame_gray, p0, None, **lk_params) p1 = p1[st == 1] # draw the tracks for pt in p1: a, b = pt.ravel() frame = cv2.circle(frame, (a, b), 5, (0, 255, 0), -1) prev_gray = frame_gray p0 = p1.reshape(-1, 1, 2) wnd.imshow(frame) key = vc.wait_key() if key == 27: break elif not tracking and key == ord('r'): # init tracking roi = cv2.selectROI('roi', frame) cv2.destroyWindow('roi') if roi is None or sum(roi) == 0: continue prev_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) p0 = cv2.goodFeaturesToTrack(prev_gray, mask=roi_mask(prev_gray, roi), **feature_params) if p0 is not None and len(p0) > 0: tracking = True video.release()
def main(): video = VideoCapture(video_sources.video_6) diff_wnd = CvNamedWindow('diff') mask_wnd = CvNamedWindow('mask') input_wnd = CvNamedWindow('input') # prev_frame = denoise(video.read()) prev_frame = cv2.cvtColor(denoise(video.read()), cv2.COLOR_BGR2GRAY) vm = VideoController(10) diff = None for frame in video.frames(): # with utils.timeit_context('frame processing'): # frame = denoise(frame) # diff = cv2.absdiff(prev_frame, frame, dst=diff) # ret, mask = cv2.threshold(diff, 45, 255, cv2.THRESH_BINARY) # binary_mask = cv2.cvtColor(mask, cv2.COLOR_BGR2GRAY) # # cn = cv2.countNonZero(binary_mask) # nonzero = cv2.findNonZero(binary_mask) with utils.timeit_context('frame processing'): frame = denoise(frame) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) diff = cv2.absdiff(prev_frame, gray, dst=diff) ret, mask = cv2.threshold(diff, 45, 255, cv2.THRESH_BINARY) # cn = cv2.countNonZero(binary_mask) nonzero = cv2.findNonZero(mask) diff_wnd.imshow(diff) mask_wnd.imshow(mask) input_wnd.imshow(utils.put_frame_pos(frame.copy(), video.frame_pos())) prev_frame = gray if not vm.wait_key(): break video.release() cv2.destroyAllWindows()
class VideoStream: def __init__(self, uri, transform=None, queue_size=128, gpu_id=None, quiet=True, read_type='numpy'): # initialize the video capture along with the boolean # used to indicate if the thread should be stopped or not self.cap = VideoCapture(uri, gpu_id=gpu_id, quiet=quiet) self.stopped = False self.transform = transform self.read_type = read_type # initialize queue and thread self.Q = Queue(maxsize=queue_size) self.thread = Thread(target=self.update, args=()) self.thread.daemon = True def start(self): # start a thread to read frames from the file video stream self.thread.start() return self def update(self): # keep looping infinitely while True: # if the thread indicator variable is set, stop the thread if self.stopped: break # otherwise, ensure the queue has room in it if not self.Q.full(): # read the next frame from the file frame = self.cap.read(type=self.read_type) # if the `grabbed` boolean is `False`, then we have # reached the end of the video file if frame is None: self.stopped = True # if there are transforms to be done, might as well # do them on producer thread before handing back to # consumer thread. ie. Usually the producer is so far # ahead of consumer that we have time to spare. # # Python is not parallel but the transform operations # are usually OpenCV native so release the GIL. # # Really just trying to avoid spinning up additional # native threads and overheads of additional # producer/consumer queues since this one was generally # idle grabbing frames. if self.transform: frame = self.transform(frame) # add the frame to the queue self.Q.put(frame) else: time.sleep(0.1) # Rest for 10ms, we have a full queue self.cap.release() def read(self): # return next frame in the queue return self.Q.get() # Insufficient to have consumer use while(more()) which does # not take into account if the producer has reached end of stream. def running(self): return self.more() or not self.stopped def more(self): # return True if there are still frames in the queue. If stream is not stopped, try to wait a moment tries = 0 while self.Q.qsize() == 0 and not self.stopped and tries < 5: time.sleep(0.1) tries += 1 return self.Q.qsize() > 0 def stop(self): # wait until stream resources are released (producer thread might be still grabbing frame) self.thread.join() # indicate that the thread should be stopped self.stopped = True
class RealSawyerLift(object): def __init__(self, control_freq=20, horizon=1000, camera_height=256,grip_thresh=.25, camera_width=256, use_image=False, use_proprio=False, port1=None, joint_proprio=True, do_sleep=True): self.joint_proprio = joint_proprio self.ctrl_freq = control_freq self.t = horizon self.use_image = use_image self.use_proprio = use_proprio self.do_sleep = do_sleep assert self.use_image or self.use_proprio, 'One of use_image and use_proprio must be set' # self.controller = controller self.cam_dim = (camera_height, camera_width) self.control_timestep = 1. / self.ctrl_freq self.is_py2 = sys.version_info[0] < 3 self.gripper_closed = 0 # 0 = open 1 = closed self.timestep = 0 self.done = False self.has_object_obs = False self.data_size = 96000 self.grip_thresh=grip_thresh self.sawyer_interface = None self.camera = None #np.random.seed(int(time.time())) self.port1 = port1 #np.random.choice(range(PORT_LOW, PORT_HIGH)) self.port2 = self.port1 + 1 self.debounce = 0 # make sure the gripper doesnt toggle too quickly self.config = make_config('RealSawyerDemoServerConfig') self.config.infer_settings() self.controller = self.config.controller.mode if self.is_py2: self.bridge = PythonBridge(self.port1, self.port2, self.data_size) self._setup_robot() self._recv_loop() else: self.bridge = PythonBridge(self.port2, self.port1, self.data_size) self._setup_camera() def signal_handler(self, signal, frame): """ Exits on ctrl+C """ if self.is_py2: self.sawyer_interface.open_gripper() if self.controller=='opspace': self.osc_controller.reset_flag.publish(True) def send(self, data): #self.close() #exit() self.bridge.send(data) def recv(self): data = self.bridge.recieve() action = array('f') action.fromstring(data) data = np.array(action) # print('Recieved', data) if data[0] == RESET and data[3] == RESET and data[-1] == RESET: self.reset() elif data[0] == SUCCESS and data[3] == SUCCESS and data[-1] == SUCCESS: return True elif data[0] == GET_POSE and data[3] == GET_POSE and data[-1] == GET_POSE: pose = self.sawyer_interface.ee_pose pose = self.sawyer_interface.pose_endpoint_transform(pose, des_endpoint='right_hand', curr_endpoint='right_gripper_tip') pose.append(self.sawyer_interface._gripper.get_position()) self.send(array('f', pose)) elif data[0] == GET_PROPRIO and data[3] == GET_PROPRIO and data[-1] == GET_PROPRIO: joint_pos = np.array(self.sawyer_interface.q) joint_vel = np.array(self.sawyer_interface.dq) gripper_pos = np.array(self.sawyer_interface._gripper.get_position()) ee_pose = self.sawyer_interface.ee_pose ee_pose = np.array(self.sawyer_interface.pose_endpoint_transform(ee_pose, des_endpoint='right_hand', curr_endpoint='right_gripper_tip')) print(joint_pos.shape, joint_vel.shape, gripper_pos.shape, ee_pose.shape) pose = np.concatenate([ np.sin(joint_pos), np.cos(joint_pos), joint_vel, [gripper_pos], [-gripper_pos], ee_pose ]) print(pose) self.send(array('f', pose)) elif data[0] == CLOSE and data[3] == CLOSE and data[-1] == CLOSE: self.close() exit() else: self._apply_action(data) def _recv_loop(self): while True: self.recv() def _setup_camera(self): self.camera = VideoCapture(0, self.cam_dim) def _setup_robot(self): self.osc_robot = make_robot(self.config.robot.type, config=self.config) self.osc_controller = make_controller(self.config.controller.type, robot=self.osc_robot, config=self.config) self.osc_controller.reset() self.osc_controller.sync_state() self.sawyer_interface = self.osc_robot.robot_arm signal.signal(signal.SIGINT, self.signal_handler) # Handles ctrl+C def reset(self): self.timestep = 0 if self.is_py2: self.sawyer_interface.open_gripper() self.sawyer_interface.reset() time.sleep(1) # Successful Reset self.send(array('f', np.array([SUCCESS] * self.dof).tolist())) return else: # Request a reset self.send(array('f', np.array([RESET] * self.dof).tolist())) _ = self.recv() return self._get_observation() #def _process_image(self, img): # h, w, _ = img.shape # new_w = int( float(w) / float(h) * self.cam_dim[0]) # new_shape = (new_w, self.cam_dim[0]) # # resized = cv2.resize(img, new_shape, cv2.INTER_AREA) # # center = new_w // 2 # left = center - self.cam_dim[1] // 2 # assume for now that this is multiple of 2 # right = center + self.cam_dim[1] // 2 # # img = resized[:,left:right,:] # img = np.array([img]) # img = np.transpose(img, (0, 3, 1, 2)) # return img def _get_image(self): ret, frame = self.camera.read() if not ret: raise RuntimeError('camera read failed') return frame def _get_proprio(self): if self.joint_proprio: self.send(array('f', np.array([GET_PROPRIO]*self.dof).tolist())) else: self.send(array('f', np.array([GET_POSE]*self.dof).tolist())) data = self.bridge.recieve() proprio = array('f') proprio.fromstring(data) return np.array(proprio) def _get_observation(self): start = time.time() di = {} if self.use_image: img = self._get_image() di['image'] = img if self.use_proprio: di['proprio'] = self._get_proprio() # print('Observation retrieval time: {}'.format(time.time()-start)) return di def _pre_action(self, action): if self.controller == 'velocity': # TODO: there is linear interpolation being done between the velocity limits # in sim, do we have something similar to that on the real robot? action = np.clip(action, -0.3, 0.3) return action def _apply_action(self, action): if self.is_py2: if self.controller == 'velocity': self.sawyer_interface.dq = action[:-1] elif self.controller == 'opspace': self.osc_controller.send_action(action[:-1]) if self.debounce: self.debounce-=1 else: if self.gripper_closed: if abs(action[-1]) < 1-self.grip_thresh: self.sawyer_interface.open_gripper() self.gripper_closed=0 self.debounce=5 print('open gripper') else: if abs(action[-1])>self.grip_thresh: self.sawyer_interface.close_gripper() self.gripper_closed=1 self.debounce=5 print('close gripper') else: # send to py2 self.send(bytes(array('f', action.tolist()))) @property def action_spec(self): low = np.ones(self.dof) * -1. high = np.ones(self.dof) * 1. return low, high @property def dof(self): return 8 def step(self, action): if self.done: raise RuntimeError('Env is terminated') start = time.time() self.timestep += 1 action = self._pre_action(action) self._apply_action(action) if self.do_sleep: end_time = start + self.control_timestep while time.time() < end_time: pass return self._get_observation(), 0., False, {} def render(self): cv2.imshow('Observation', self._get_image()) cv2.waitKey(1) def observation_spec(self): observation = self._get_observation() return observation def close(self): if self.is_py2: self.sawyer_interface.open_gripper() self.sawyer_interface.reset() else: self.camera.release() self.send(array('f', np.array([CLOSE] * self.dof).tolist()))