def main():
Timer.enable()
with CameraReader(1) as cap:
k_dilate = cv2.getStructuringElement(cv2.MORPH_DILATE, (5, 5), (3, 3))
dims = (480, 640)
dims3 = dims + (3, )
frame = np.empty(dims3, dtype=np.uint8)
thresh = np.empty(dims, dtype=np.uint8)
blur = np.empty(dims3, dtype=np.uint8)
hsv = np.empty(dims3, dtype=np.uint8)
gray = np.empty(dims, dtype=np.uint8)
segments = np.empty(dims, dtype=np.uint8)
superpixel = np.empty(dims3, dtype=np.uint8)
shifted = np.empty(dims3, dtype=np.float32)
s = SuperPixel(200)
# setup matrices
while True:
with Timer('Cap'):
cap.read(frame)
with Timer('SuperPixel'):
superpixel = s.apply(frame)
cv2.imshow('frame', frame)
cv2.imshow('slic', superpixel)
if cv2.waitKey(10) == 27: # ESC
break
def process(self, data):
with Timer('superpixelnode_1'):
im_l, im_r = data
if self.rgb is None:
self.rgb = np.empty_like(im_l)
cv2.cvtColor(im_l, cv2.COLOR_BGR2RGB, dst=self.rgb)
labels = slic(self.rgb, self.n_segments)
return im_l, im_r, self.rgb, labels
def process(self, data):
with Timer('superpixelnode_2'):
im_l, im_r, rgb, labels = data
edges = filters.sobel(color.rgb2gray(rgb))
g = graph.rag_boundary(labels, edges)
labels2 = graph.merge_hierarchical(labels,
g,
thresh=0.02,
rag_copy=False,
in_place_merge=True,
merge_func=merge_boundary,
weight_func=weight_boundary)
return im_l, im_r, labels2.astype(np.uint8)
def test_matcher():
with CameraReader(1) as cap_l, CameraReader(2) as cap_r:
while True:
m = SuperPixelMatcher(400)
dims = (480, 640)
dims3 = dims + (3, )
frame_l = np.empty(dims3, dtype=np.uint8)
frame_r = np.empty(dims3, dtype=np.uint8)
with Timer('Cap'):
cap_l.read(frame_l)
cap_r.read(frame_r)
with Timer('SuperPixel'):
pxl_l, pxl_r, disp = m.apply(frame_l, frame_r)
cv2.imshow('l', pxl_l)
cv2.imshow('r', pxl_r)
cv2.imshow('d', disp)
if cv2.waitKey(100) == 27:
break
def process(self, data):
with Timer('superpixelnode_3'):
im_l, im_r, segments = data
if self.lab is None:
self.lab = np.empty_like(im_l)
sp_img = np.zeros_like(im_l)
ptss = []
cv2.cvtColor(im_l, cv2.COLOR_BGR2LAB, dst=self.lab)
for i in range(1 + np.max(segments)):
pts = np.where(segments == i)
ptss.append(pts)
lab_t = np.average(self.lab[pts[0], pts[1]], axis=0).reshape(
(1, 1, 3)).astype(np.uint8)
t = cv2.cvtColor(lab_t, cv2.COLOR_LAB2BGR).reshape(3)
sp_img[pts] = t
return im_l, im_r, ptss, sp_img
def apply(self, frame, dst=None):
#with Timer('super-segments'):
segments = self.get_segments(frame)
if dst is None:
dst = self.superpixel
with Timer('super-fill'):
dst.fill(0)
self.pts = []
cv2.cvtColor(frame, cv2.COLOR_BGR2LAB, dst=self.lab)
for i in range(1 + np.max(segments)):
pts = np.where(segments == i)
self.pts.append(pts)
### "Better" Mean Color ...
lab_t = np.average(self.lab[pts[0], pts[1]], axis=0).reshape(
(1, 1, 3)).astype(np.uint8)
t = cv2.cvtColor(lab_t, cv2.COLOR_LAB2BGR).reshape(3)
#t = np.average(frame[pts[0], pts[1]], axis=0).astype(np.uint8)
###
dst[pts] = t
return dst
def demo():
global click
Timer.enable(True)
setup_windows()
rospy.init_node('object_tracker')
pcl = rospy.get_param('pcl', default=True)
print 'pcl', pcl
# initialize rectifier
rospack = rospkg.RosPack()
pkg_root = rospack.get_path('edwin')
bridge = CvBridge()
rectifier = Rectifier(
param_l = os.path.join(pkg_root, 'Stereo/camera_info/left_camera.yaml'),
param_r = os.path.join(pkg_root, 'Stereo/camera_info/right_camera.yaml')
)
obj_pub = rospy.Publisher('obj_point', PointStamped, queue_size=1)
obj_msg = PointStamped()
obj_msg.header.frame_id = 'camera_link'
if pcl:
pcl_pub = rospy.Publisher('point_cloud', PointCloud2, queue_size=1)
pcl_msg = PointCloud2()
### SETUP IMAGES
dims = (480,640)
dims3 = (480,640,3)
left = np.empty(dims3, dtype=np.uint8)
right = np.empty(dims3, dtype=np.uint8)
im_l = np.empty(dims3,dtype=np.uint8)
im_r = np.empty(dims3,dtype=np.uint8)
sp_l = np.empty(dims3,dtype=np.uint8)
superpixel = SuperPixel(800)
nodes = [SuperPixelNode1(400), SuperPixelNode2(), SuperPixelNode3(), DisparityNode()]
pipeline = Pipeline(nodes)
with CameraReader(1) as cam_l, CameraReader(2) as cam_r:
def input_fn():
cam_l.read(left)
cam_r.read(right)
rectifier.apply(left, right, dst_l=im_l, dst_r=im_r)
return left, right
def output_fn(x):
sp_l, disp = x
cv2.imshow('sp_l', sp_l)
cv2.imshow('disp', disp)
raw_dist = cv2.reprojectImageTo3D((disp/16.).astype(np.float32), rectifier.Q, handleMissingValues=True)
dist = raw_dist[:,:,2]
if pcl:
pcl_msg = toPointCloud(raw_dist, im_l)
pcl_pub.publish(pcl_msg)
cv2.imshow('dist', dist)
params = {
'color' : 'yellow',
'min_dist' : 0.3,
'max_dist' : 1.5,
'min_area' : 0.003,
'max_area' : 1.0
}
filtered, ctrs = apply_criteria(dist, sp_l, params)
if len(ctrs) > 0:
m = cv2.moments(ctrs[0]) # pull out biggest one--ish
cX = int(m["m10"] / m["m00"])
cY = int(m["m01"] / m["m00"])
x,y,z = raw_dist[cY,cX]
x,y,z = z,-x, -y
obj_msg.header.stamp = rospy.Time.now()
obj_msg.point.x = x
obj_msg.point.y = y
obj_msg.point.z = z
obj_pub.publish(obj_msg)
cv2.imshow('filtered', filtered)
pipeline.run(input_fn, output_fn)
#with CameraReader(1) as cam_l, CameraReader(2) as cam_r:
##with ROSCameraReader('/edwin_camera/left/image_raw') as cam_l, \
## ROSCameraReader('/edwin_camera/right/image_raw') as cam_r:
# #matcher = Matcher()
# #tracker = ObjectTracker()
# #manager = ObjectManager()
# #tracker.set_target('medium','blue')
# while not rospy.is_shutdown():
# cam_l.read(left)
# cam_r.read(right)
# rectifier.apply(left, right, dst_l=im_l, dst_r=im_r)
# with Timer('SuperPixel'):
# superpixel.apply(im_l, dst=sp_l)
# cv2.imshow('sp_l', sp_l)
# with Timer('Disparity'):
# disp = handle_disp(im_l, im_r, rectifier.Q)
# #im_disp = cv2.normalize(disp,None,0.0,255.0,cv2.NORM_MINMAX).astype(np.uint8)
# #cv2.imshow('im_disp', im_disp)
# disp = apply_superpixel(disp, superpixel)
# raw_dist = cv2.reprojectImageTo3D((disp/16.).astype(np.float32), rectifier.Q, handleMissingValues=True)
# if pcl:
# with Timer('PCL'):
# pcl_msg = toPointCloud(raw_dist, im_l)
# pcl_pub.publish(pcl_msg)
# dist = raw_dist[:,:,2]
# cv2.imshow('dist', dist)
# #blur = cv2.GaussianBlur(im_l,(3,3),0)
# #im_opt = handle_opt(blur)
# #im_bksub = handle_bksub(blur)
# identified = im_l.copy()
# params = {
# 'color' : 'yellow',
# 'min_dist' : 0.3,
# 'max_dist' : 1.5,
# 'min_area' : 0.003,
# 'max_area' : 1.0
# }
# filtered, ctrs = apply_criteria(dist, sp_l, params)
# if len(ctrs) > 0:
# m = cv2.moments(ctrs[0]) # pull out biggest one--ish
# cX = int(m["m10"] / m["m00"])
# cY = int(m["m01"] / m["m00"])
# x,y,z = raw_dist[cY,cX]
# x,y,z = z,-x, -y
# obj_msg.header.stamp = rospy.Time.now()
# obj_msg.point.x = x
# obj_msg.point.y = y
# obj_msg.point.z = z
# obj_pub.publish(obj_msg)
# cv2.imshow('filtered', filtered)
# if cv2.waitKey(10) == 27:
# break
cv2.destroyAllWindows()
def process(self,data):
with Timer('disparity_node'):
im_l, im_r, pts, sp_l = data
disp = self.bm.apply(im_l, im_r)
disp = apply_superpixel(disp,pts)
return sp_l, disp