def see(self,
mode,
arena,
res=None,
stats=False,
save=True,
fast_capture=True,
zone=0):
if isinstance(self.camera, picamera.PiCamera):
if res is not None and res not in picamera_focal_lengths:
raise ValueError("Invalid resolution: {}".format(res))
else:
if res is not None and res not in usbcamera_focal_lengths:
raise ValueError("Invalid resolution: {}".format(res))
self.lock.acquire()
if res is not None:
self._set_res(res)
acq_time = time.time()
timer = Timer()
times = {}
with timer:
if isinstance(self.camera, picamera.PiCamera):
with picamera.array.PiRGBArray(self.camera) as stream:
self.camera.capture(stream,
format="bgr",
use_video_port=fast_capture)
if save:
cv2.imwrite("/tmp/colimage.jpg", stream.array)
image = cv2.cvtColor(stream.array, cv2.COLOR_BGR2GRAY)
else:
frame = self.camera.get_frame()
times["cam"] = timer.time
with timer:
if isinstance(self.camera, picamera.PiCamera):
if os.path.exists('/media/RobotUSB/collect_images.txt'):
filename = "/media/RobotUSB/" + str(int(acq_time)) + ".jpg"
cv2.imwrite(filename, image)
# Create an IplImage header for the image.
# (width, height), depth, num_channels
ipl_image = cv2.cv.CreateImageHeader(
(image.shape[1], image.shape[0]), cv2.cv.IPL_DEPTH_8U, 1)
# Put the actual image data in the IplImage.
# The third argument is the row length ("step").
# Note that pykoki will automatically free `ipl_image` after the markers are obtained from it.
cv2.cv.SetData(ipl_image, image.tobytes(),
image.dtype.itemsize * image.shape[1])
# Make sure the image data is actually in the IplImage. Don't touch this line!
ipl_image.tostring()
else:
ipl_image = self.koki.v4l_YUYV_frame_to_grayscale_image(
frame, *self._res)
times["manipulation"] = timer.time
# Now that we're dealing with a copy of the image, release the camera lock
self.lock.release()
if isinstance(self.camera, picamera.PiCamera):
params = CameraParams(
Point2Df(self.camera.resolution[0] / 2,
self.camera.resolution[1] / 2),
Point2Df(*picamera_focal_lengths[self.camera.resolution]),
Point2Di(*self.camera.resolution))
else:
params = CameraParams(
Point2Df(self._res[0] / 2, self._res[1] / 2),
Point2Df(*usbcamera_focal_lengths[self._res]),
Point2Di(*self._res))
with timer:
markers = self.koki.find_markers_fp(
ipl_image,
functools.partial(self._width_from_code,
marker_luts[mode][arena]), params)
times["find_markers"] = timer.time
srmarkers = []
usb_log = os.path.exists("/media/RobotUSB/log_markers.txt")
if markers and usb_log:
logfile = open("/media/RobotUSB/" + str(int(acq_time)) + ".txt",
"w")
for m in markers:
if usb_log:
# noinspection PyUnboundLocalVariable
logfile.write(
"code: {}, distance: {}, rot_x: {}, rot_y: {}\n".format(
m.code, round(m.distance, 3), round(m.bearing.x, 3),
round(m.bearing.y, 3)))
if m.code not in marker_luts[mode][arena][zone]:
# Ignore other sets of codes
if usb_log:
logfile.write(
"(last marker not part of competition, ignoring)\n")
continue
info = marker_luts[mode][arena][zone][int(m.code)]
vertices = []
for v in m.vertices:
vertices.append(
Point(
image=ImageCoord(x=v.image.x, y=v.image.y),
world=WorldCoord(x=v.world.x, y=v.world.y,
z=v.world.z),
# libkoki does not yet provide these coords
polar=PolarCoord(0, 0, 0)))
num_quarter_turns = int(m.rotation_offset / 90)
num_quarter_turns %= 4
vertices = vertices[
num_quarter_turns:] + vertices[:num_quarter_turns]
centre = Point(image=ImageCoord(x=m.centre.image.x,
y=m.centre.image.y),
world=WorldCoord(x=m.centre.world.x,
y=m.centre.world.y,
z=m.centre.world.z),
polar=PolarCoord(length=m.distance,
rot_x=m.bearing.x,
rot_y=m.bearing.y))
orientation = Orientation(rot_x=m.rotation.x,
rot_y=m.rotation.y,
rot_z=m.rotation.z)
marker = Marker(info=info,
timestamp=acq_time,
res=res,
vertices=vertices,
centre=centre,
orientation=orientation)
srmarkers.append(marker)
if markers and usb_log:
logfile.close()
if not isinstance(self.camera, picamera.PiCamera):
self.koki.image_free(ipl_image)
if stats:
return srmarkers, times
return srmarkers
def see(self, mode, arena, res, stats):
self.lock.acquire()
self._set_res(res)
acq_time = time.time()
timer = Timer()
times = {}
with timer:
frame = self.camera.get_frame()
times["cam"] = timer.time
with timer:
img = self.koki.v4l_YUYV_frame_to_grayscale_image(
frame, self._res[0], self._res[1])
times["yuyv"] = timer.time
# Now that we're dealing with a copy of the image, release the camera lock
self.lock.release()
params = CameraParams(Point2Df(self._res[0] / 2, self._res[1] / 2),
Point2Df(*self.camera_focal_length[self._res]),
Point2Di(*self._res))
with timer:
markers = self.koki.find_markers_fp(
img,
functools.partial(self._width_from_code,
marker_luts[mode][arena]), params)
times["find_markers"] = timer.time
srmarkers = []
for m in markers:
if m.code not in marker_luts[mode][arena]:
"Ignore other sets of codes"
continue
info = marker_luts[mode][arena][int(m.code)]
vertices = []
for v in m.vertices:
vertices.append(
Point(
image=ImageCoord(x=v.image.x, y=v.image.y),
world=WorldCoord(x=v.world.x, y=v.world.y,
z=v.world.z),
# libkoki does not yet provide these coords
polar=PolarCoord(0, 0, 0)))
num_quarter_turns = int(m.rotation_offset / 90)
num_quarter_turns %= 4
vertices = vertices[
num_quarter_turns:] + vertices[:num_quarter_turns]
centre = Point(image=ImageCoord(x=m.centre.image.x,
y=m.centre.image.y),
world=WorldCoord(x=m.centre.world.x,
y=m.centre.world.y,
z=m.centre.world.z),
polar=PolarCoord(length=m.distance,
rot_x=m.bearing.x,
rot_y=m.bearing.y))
orientation = Orientation(rot_x=m.rotation.x,
rot_y=m.rotation.y,
rot_z=m.rotation.z)
marker = Marker(info=info,
timestamp=acq_time,
res=res,
vertices=vertices,
centre=centre,
orientation=orientation)
srmarkers.append(marker)
self.koki.image_free(img)
if stats:
return (srmarkers, times)
return srmarkers
#!/usr/bin/env python
from pykoki import PyKoki, Point2Di, Point2Df, CameraParams
import cv, cv2
import sys
if len(sys.argv) != 2:
print >> sys.stderr, "opencv_example.py IMG_FILE"
exit(1)
img = cv.LoadImage(sys.argv[1], cv.CV_LOAD_IMAGE_GRAYSCALE)
koki = PyKoki()
params = CameraParams(Point2Df(img.width / 2, img.height / 2),
Point2Df(571, 571), Point2Di(img.width, img.height))
print koki.find_markers(img, 0.1, params)