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 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()