def display_opencv(robot: cozmo.robot.Robot):
# params
camK = np.matrix([[295, 0, 160], [0, 295, 120], [0, 0, 1]],
dtype='float32')
marker_size = 4.8
# start streaming
robot.camera.image_stream_enabled = True
while True:
latest_image = robot.world.latest_image
if latest_image is not None:
# convert pil to opencv
open_cv_image = np.array(latest_image.raw_image)
#detect markers
markers = detect_markers(open_cv_image, marker_size, camK)
for marker in markers:
marker.highlite_marker(open_cv_image,
draw_frame=True,
camK=camK)
#print("ID =", marker.id);
#print(marker.contours);
cv2.imshow("Markers", open_cv_image)
cv2.waitKey(1)
m2d_list = cvt_marker_measurements(markers)
print(m2d_list)
async def image_processing(robot):
global camK, marker_size
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
opencv_image = np.asarray(event.image)
markers = detect_markers(opencv_image, marker_size, camK)
for marker in markers:
marker.highlite_marker(opencv_image, draw_frame=True, camK=camK)
cv2.imshow("Markers", opencv_image)
return markers
async def image_processing(robot):
global camK, marker_size
event = await robot.world.wait_for(cozmo.camera.EvtNewRawCameraImage, timeout=30)
# convert camera image to opencv format
opencv_image = np.asarray(event.image)
# detect markers
markers = detect_markers(opencv_image, marker_size, camK)
# show markers
for marker in markers:
marker.highlite_marker(opencv_image, draw_frame=True, camK=camK)
# print("ID =", marker.id);
# print(marker.contours);
cv2.imshow("Markers", opencv_image)
return markers
def get_target_pose(self, frame):
markers = detect_markers(frame, BASE_MARKER)
if not markers:
return None, None
marker = markers[0]
raw_points = [marker.contours[i][0] for i in xrange(4)]
rot_points = []
for i in xrange(4):
j = (i - marker.rotation) % 4
rot_points.append(raw_points[j])
rot_points = np.array(rot_points, dtype=np.float32)
_, rvec, tvec = cv2.solvePnP(np.array([self.marker_frame]),
np.array([rot_points]),
self.K,
self.d,
flags=cv2.CV_ITERATIVE)
return rvec, tvec
def get_matrix(img_data):
img = convert_image(img_data)
dm = detect_markers(img)
points = []
point_ids = []
for marker in dm:
if marker.id in current_app.config['MARKERS'] and not marker.id in point_ids:
#marker.highlite_marker(img)
point_ids.append(marker.id)
points.append({ 'id': marker.id, 'x': marker.center[0], 'y': marker.center[1] })
#print 'Markers found: %s' % len(points)
if len(points) > 2:
#for p in points:
# print p['id']
#print '------'
return calc_transform(points)
return None
def onImage(self, cv2_image):
""" This method gets called upon receiving an input image given by cv2_image.
The method detects the markers. Then it chooses one HammingMarker object for each recognized
marker id and draws it on the output image. Afterwards the additional information is send to
the corresponding ports.
@param cv2_image - an OpenCV image object
"""
# we only care for one contour
markers = {}
for marker in detect_markers(cv2_image):
markers[marker.id] = marker
marker_list = [markers[mid] for mid in markers]
# handle memory
if self.memory_length > 0:
# set all current marker information
cur_time = time.time()
for marker in marker_list:
self.memory[marker.id] = (marker, cur_time)
# create new marker list and only include marker which are within the time frame
marker_list = [
self.memory[mid][0] for mid in self.memory
if cur_time - self.memory[mid][1] < self.memory_length
]
# highlight markers in output image
for marker in marker_list:
marker.highlite_marker(cv2_image)
self.sendMarkers(marker_list)
self.sendOrder(marker_list)
self.sendTranslation(marker_list)
return cv2_image
def onImage(self, cv2_image):
""" This method gets called upon receiving an input image given by cv2_image.
The method detects the markers. Then it chooses one HammingMarker object for each recognized
marker id and draws it on the output image. Afterwards the additional information is send to
the corresponding ports.
@param cv2_image - an OpenCV image object
"""
# we only care for one contour
markers = {}
for marker in detect_markers(cv2_image):
markers[marker.id] = marker
marker_list = [markers[mid] for mid in markers]
# handle memory
if self.memory_length > 0:
# set all current marker information
cur_time = time.time()
for marker in marker_list:
self.memory[marker.id] = (marker, cur_time)
# create new marker list and only include marker which are within the time frame
marker_list = [
self.memory[mid][0] for mid in self.memory if cur_time - self.memory[mid][1] < self.memory_length
]
# highlight markers in output image
for marker in marker_list:
marker.highlite_marker(cv2_image)
self.sendMarkers(marker_list)
self.sendOrder(marker_list)
self.sendTranslation(marker_list)
return cv2_image
from ar_markers.hamming.detect import detect_markers
if __name__ == '__main__':
debug = False
if "-d" in sys.argv:
debug = True
# for line in sys.stdin:
capture = cv2.VideoCapture(0)
if capture.isOpened():
frame_captured, frame = capture.read()
else:
frame_captured = False
while frame_captured:
markers = detect_markers(frame)
for marker in markers:
marker.highlite_marker(frame)
print marker.id
sys.stdout.flush()
if debug:
cv2.imshow('Test Frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
frame_captured, frame = capture.read()
# When everything done, release the capture
capture.release()
cv2.destroyAllWindows()
