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
