def get_corners_from_pc(pc,rows=6,cols=8): xyz, rgb = ru.pc2xyzrgb(pc) rgb = np.copy(rgb) rtn, corners = get_corners_rgb(rgb, rows, cols) if len(corners) == 0: return 0, None points = get_xyz_from_corners(corners, xyz) return rtn, points
def view_pointclouds(bag, buffer_size=3, cloud_topics=None): if cloud_topics is None: cloud_topics = ['/camera1/depth_registered/points', '/camera2/depth_registered/points'] prev_buffers = {1:[],2:[]} times = [] ros_times = [] for (topic, cloud, time) in bag.read_messages(topics=cloud_topics): # assert cloud.header.stamp.to_nsec() == time.to_nsec() xyz, rgb = ru.pc2xyzrgb(cloud) rgb = rgb.copy() t = cloud.header.stamp.to_sec() times.append(t) ros_times.append(time.to_sec()) if topic.find('1') >= 0: prev_buffer = prev_buffers[1] else: prev_buffer = prev_buffers[2] if len(prev_buffer) >= buffer_size: prev_buffer.pop(0) prev_buffer.append((rgb,cloud.header.stamp.to_sec())) curr_pos = len(prev_buffer) - 1 while True: cv2.imshow(WIN_NAME, rgb) print 'Time:', t key = cv2.waitKey() if key == 65363: curr_pos += 1 elif key == 65361: curr_pos -= 1 else: continue if curr_pos >= len(prev_buffer): break if curr_pos < 0: redprint("Reached end of buffer. Buffersize = %i"%buffer_size) curr_pos = 0 rgb,t = prev_buffer[curr_pos] ppl.plot(times,'s', markersize=5) ppl.hold(True) ppl.plot(ros_times,'ro', markersize=5) ppl.show()
def get_corners_from_pc(pc,method='pycb', rows=None,cols=None): xyz, rgb = ru.pc2xyzrgb(pc) rgb = np.copy(rgb) rtn, corners = get_corners_rgb(rgb, method, rows, cols) if rtn == 1: points = get_xyz_from_corners(corners, xyz) return rtn, points if len(corners) == 0 or rtn == 0: return 0, None if method=='pycb': allpoints = [] for cor in corners: cor = cor.reshape(cor.shape[0]*cor.shape[1],2,order='F') points.append(get_zyx_from_corners(cor, xyz)) return rtn, points
def process_observation_pycb(self): """ Get an observation and update transform list. """ sleeper = rospy.Rate(10) for j in xrange(self.num_cameras): pc = self.cameras.get_pointcloud(j) xyz, rgb = ru.pc2xyzrgb(pc) rgb = np.copy(rgb) _, corners = cu.get_corners_rgb(rgb, method='cv', rows=chessboard_rows, cols=chessboard_cols) if len(corners) < chessboard_cols * chessboard_rows * 1.0 / 3.0: redprint ("Found too few corners: %i" % len(corners)) return False self.image_list[j].append(rgb) return True
def pc_callback(self, msg): self.latest_pc = msg _, self.latest_img = ru.pc2xyzrgb(msg) self.latest_img = np.copy(self.latest_img)
def get_markers_kinect_ros(): import rospy from sensor_msgs.msg import PointCloud2 rospy.init_node('get_pc') class msg_storer: def __init__(self): self.last_msg = None def callback(self,msg): self.last_msg = msg def mouse_click(event, x, y, flags, params): if event == cv.CV_EVENT_LBUTTONDOWN: clicks.append([x, y]) def find_avg(points, clicks, cloud): avgPoints = [] rad = 0.02 r = 5 for (x,y), point in zip(clicks,points): avg = np.array([0,0,0]) num = 0.0 for i in range(-r,r+1): for j in range(-r,r+1): cloudPt = cloud[y+i, x+j,:]; if not np.isnan(cloudPt).any() and np.linalg.norm(point - cloudPt) < rad: avg = avg + cloudPt num += 1.0 if num == 0.0: print "not found" avgPoints.append(point) else: print "found" avgPoints.append(avg / num) return avgPoints pc_storer = msg_storer() rospy.Subscriber('/camera/depth_registered/points', PointCloud2, callback=pc_storer.callback) wait = rospy.Rate(10) while pc_storer.last_msg == None: print "waiting for point cloud" wait.sleep() pc = pc_storer.last_msg xyz, rgb = ros_utils.pc2xyzrgb(pc) arr = np.fromstring(pc.data,dtype='float32').reshape(pc.height,pc.width,pc.point_step/4) rgb0 = np.ndarray(buffer=arr[:,:,4].copy(),shape=(pc.height, pc.width,4),dtype='uint8') print arr.shape, arr.dtype print rgb0.shape, rgb0.dtype print xyz.shape, xyz.dtype print rgb.shape, rgb.dtype clicks = [] hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV) h = hsv[:,:,0] s = hsv[:,:,1] v = hsv[:,:,2] red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100) #cv2.imshow("red",red_mask.astype('uint8')*255) cv2.imshow("red", red_mask.astype('uint8')*255) cv2.imshow("rgb", rgb) cv.SetMouseCallback("rgb", mouse_click, 0) print "press enter to continue" cv2.waitKey() clickPoints = [] #get click points for (x, y) in clicks: clickPoints.append(np.copy(xyz[y][x])) # import IPython # IPython.embed() red_mask3d = np.tile(np.atleast_3d(red_mask), [1,1,3]) np.putmask (xyz, red_mask3d != 1, np.NAN) print clicks print clickPoints avgPoints = find_avg(clickPoints, clicks, xyz) print avgPoints return avgPoints