def rangeGUI():
""" creates and displays a GUI for the range finder data
Ranges window: shows range finder values as red lines
coming from the center of the range finder
HoughLines window: shows the result of using a Hough
transformation on image containing the range values as points.
"""
global D
init_GUI() # initialize images and windows
D.dangerList = [] # Set up the danger point list
# main loop
while rospy.is_shutdown() == False:
# loop through every angle
for angle in range(REV):
magnitude = MAG_SCALE * D.ranges[angle]
x = int(CENTER + magnitude * cos(pi / 180 * (angle + ANGLE_OFFSET))
) # find x and y coordinates based on the angle
y = int(CENTER - magnitude *
sin(pi / 180 *
(angle + ANGLE_OFFSET))) # and the length of the line
# put the danger points into the list
if x > CENTER - 30 and x < CENTER + 30 and y < CENTER - 30 and y > CENTER - 90:
D.dangerList.append((x, y))
if 0 < magnitude < MAX_MAG: # if data is within "good data" range
# add line to the ranges image
cv.Line(D.image, (CENTER, CENTER), (x, y), cv.RGB(255, 0, 0))
# add dot to image being used in Hough transformation
cv.Line(D.hough, (x, y), (x, y), cv.RGB(255, 0, 0))
# wait and check for quit request
key_code = cv.WaitKey(1) & 255
key_press = chr(key_code)
if key_code == 27 or key_press == 'q': # if ESC or 'q' was pressed
rospy.signal_shutdown("Quitting...")
# find walls and add to image using Hough transformation
findHoughLines()
# show image with range finder data and calculated walls
cv.ShowImage("Ranges", D.image)
# show image used in Hough transformation if option is selected
if SHOW_HOUGH:
cv.ShowImage("HoughLines", D.color_dst)
# clear the images for next loop
cv.Set(D.image, cv.RGB(0, 0, 0))
cv.Set(D.hough, cv.RGB(0, 0, 0))
# clear the danger list for next loop
D.dangerList = []
D.tank(0, 0) # stops the robot
print "Quitting..."
def draw_crosshair(self):
hcenter = self.image.width / 2 + self.yoffset
vcenter = self.image.height / 2 + self.xoffset
#draw horizontal Line
cv.Line(self.image, (0, vcenter), (self.image.width, vcenter),
cv.RGB(255, 0, 0), self.stroke, cv.CV_AA, 0)
#draw vertical Line
cv.Line(self.image, (hcenter, 30), (hcenter, self.image.height),
cv.RGB(255, 0, 0), self.stroke, cv.CV_AA, 0)
#draw upper half-circles
for i in range(0, 4):
cv.Ellipse(self.image, (hcenter, vcenter - self.stretch),
(self.dia * i, self.dia * i), 0, 180, 360,
cv.RGB(255, 0, 0), self.stroke, cv.CV_AA, 0)
#draw connecting lines id streched
if self.stretch > 0:
for i in range(1, 4):
cv.Line(self.image,
(hcenter + self.dia * i, vcenter - self.stretch),
(hcenter + self.dia * i, vcenter + self.stretch),
cv.RGB(255, 0, 0), self.stroke, cv.CV_AA, 0)
cv.Line(self.image,
(hcenter - self.dia * i, vcenter - self.stretch),
(hcenter - self.dia * i, vcenter + self.stretch),
cv.RGB(255, 0, 0), self.stroke, cv.CV_AA, 0)
#draw lower half-circles
for i in range(0, 4):
cv.Ellipse(self.image, (hcenter, vcenter + self.stretch),
(self.dia * i, self.dia * i), 180, 180, 360,
cv.RGB(255, 0, 0), self.stroke, cv.CV_AA, 0)
def visualize_points(self, filename, ex_points, te_points, image):
"""
Show the transformed rectangle where the best match of the object name
was found
"""
#Visualize keypoints in training image:
for point in range(len(ex_points)):
thickness = 2
color2 = (0,255,0)
x = int(round(ex_points[point][0][0]))
y = int(round(ex_points[point][0][1]))
cv.Line(self.images[filename], (x-1,y), (x+1,y), color2, thickness)
cv.Line(self.images[filename], (x,y-1), (x,y+1), color2, thickness)
#Visualize keypoints in testing image:
for point in range(len(te_points)):
try:
thickness = 2
color2 = (0,255,0)
x = te_points[point][0]
y = te_points[point][1]
cv.Line(image, (x-1,y), (x+1,y), color2, thickness)
cv.Line(image, (x,y-1), (x,y+1), color2, thickness)
except:
pass
def draw_panel(cv,
img,
x,
y,
w,
h,
xsubdiv=10,
ysubdiv=2,
color=(255, 0, 255),
vals=None):
# http://docs.opencv.org/modules/core/doc/drawing_functions.html
xs = w / float(xsubdiv)
ys = h / float(ysubdiv)
for i in range(0, xsubdiv + 1):
xx = int(x + i * xs + 0.5)
cv.Line(img, (xx, int(y + .5)), (xx, int(y + h + .5)), color, 1)
for i in range(0, ysubdiv + 1):
yy = int(y + i * ys + 0.5)
cv.Line(img, (int(x + .5), yy), (int(x + w + .5), yy), color, 1)
if (vals):
l = list(reversed(vals))
s = int(img.width * 2 / (3 * xsubdiv))
ww = int(s * .8)
for i in range(xsubdiv):
xx = int(0.2 * ww + i * s)
for j in range(ysubdiv):
yy = int(img.height - (ysubdiv - j) * s)
c = l.pop() # unshift, actually.
cv.Rectangle(img, (xx, yy), (xx + ww, yy + ww),
(c[0], c[1], c[2]), cv.CV_FILLED)
def paint_estimates(self, img):
"paints the camera estimates"
circles = []
if 'cam1' in self._draw_estimates:
cv.Circle(img, self.cam1_estimate, 10, kc.cam1color, -1)
cv.Line(img, self.cam1_estimate, self.cam1center, kc.cam1color, 1, cv.CV_AA)
circles.append({'color':kc.cam1color, 'text':'Camera 1'})
if 'cam2' in self._draw_estimates:
cv.Circle(img, self.cam2_estimate, 10, kc.cam2color, -1)
cv.Line(img, self.cam2_estimate, self.cam2center, kc.cam2color, 1, cv.CV_AA)
circles.append({'color':kc.cam2color, 'text':'Camera 2'})
# cv.Circle(img, self.cam3_estimate, 10, kc.cam3color, -1)
# cv.Line(img, self.cam3_estimate, self.cam3center, kc.cam3color, 1, cv.CV_AA)
if 'kalman' in self._draw_estimates:
cv.Circle(img, self.kalman_estimate, 10, kc.kalmancolor, -1)
circles.append({'color':kc.kalmancolor, 'text':'Kalman'})
if 'average' in self._draw_estimates:
cv.Circle(img, self.average_estimate, 10, kc.averagecolor, -1)
circles.append({'color':kc.averagecolor, 'text':'Average'})
circles.reverse()
for cnt,c in enumerate(circles):
yloc = kc.img_size[1] - 20 - cnt*25
cloc = (200, yloc)
cv.Circle(img, cloc, 10, c['color'], -1)
tloc = (220, yloc+5)
cv.PutText(img, c['text'], tloc, self._font, kc.font_color)
def CompositeShow(windowName, camera, image, settings, pts=[]):
global Uncalibrated
cv.NamedWindow(windowName)
comp = cv.CloneImage(image)
if (Uncalibrated):
CalibrateCameras(comp)
Uncalibrated = False
if (settings.showGrid):
#draw lines
#p1 - p2
cv.Line(comp, tuple(camera.calibrationmarkers[0].pos), \
tuple(camera.calibrationmarkers[1].pos), cv.Scalar(0, 255, 0), 1, cv.CV_AA)
#p1 - p4
cv.Line(comp, tuple(camera.calibrationmarkers[0].pos), \
tuple(camera.calibrationmarkers[3].pos), cv.Scalar(0, 255, 0), 1, cv.CV_AA)
#p3 - p4
cv.Line(comp, tuple(camera.calibrationmarkers[2].pos), \
tuple(camera.calibrationmarkers[3].pos), cv.Scalar(0, 255, 0), 1, cv.CV_AA)
#p2 - p3
cv.Line(comp, tuple(camera.calibrationmarkers[1].pos), \
tuple(camera.calibrationmarkers[2].pos), cv.Scalar(0, 255, 0), 1, cv.CV_AA)
for pt in pts:
cv.Circle(comp, (int(pt[0]), int(pt[1])), 3, cv.Scalar(255, 0, 0))
cv.ShowImage(windowName, comp)
def draw_ent(self, ent):
if not ent: return
x, y = intPoint(ent.pos)
radius = 30
#cv.Circle(self.image, intPoint((x,y)), 8, color, -1)
o = ent.orientation
D = 30
cv.Circle(self.image, intPoint((x + D * cos(o), y + D * sin(o))), 6,
(200, 200, 200), -1)
# Draw estimated robot boundaries
points = ent.points
for p1, p2 in zip(points[1:] + points[:1], points[:-1] + points[-1:]):
cv.Line(self.image, intPoint(p1), intPoint(p2), (200, 200, 0), 1,
cv.CV_AA)
cv.Circle(self.image, intPoint(p1), 2, (255, 255, 100), -1)
if ent.text is not None:
x, y = (x - 42, y + 42)
for i, line in enumerate(ent.text):
cv.PutText(self.image, line, (x, y + 12 * i), self.Font,
(0, 200, 200))
if ent.target is not None:
cv.Line(self.image, intPoint(ent.pos), intPoint(ent.target),
(180, 0, 180), 1, cv.CV_AA)
def draw_vertical_sample_boundaries(curr_image_cv, warp_matrix, square_length):
#TODO: IF THIS IS EVER USED, DO NOT USE THIS VALUE
assert (False)
scale_factor = 0.5
curr_image = np.array(curr_image_cv)
full_width = curr_image.shape[1]
full_height = curr_image.shape[0]
width = int(curr_image.shape[1] * scale_factor)
height = int(curr_image.shape[0] * scale_factor)
full_left_bound = int(full_width * (0.5 - square_length / 2))
full_right_bound = int(full_width * (0.5 + square_length / 2))
left_bound = int(full_left_bound * scale_factor)
right_bound = int(full_right_bound * scale_factor)
full_line1 = ((full_left_bound, 0), (full_left_bound, full_height - 1))
full_line2 = ((full_right_bound, 0), (full_right_bound, full_height - 1))
line1 = ((left_bound, 0), (left_bound, height - 1))
line2 = ((right_bound, 0), (right_bound, height - 1))
if curr_image.ndim == 2:
sample_image = curr_image[:, :, np.newaxis]
sample_image = np.tile(sample_image, (1, 1, 3))
sample_image = cv.fromarray(sample_image)
resized_sample_image = cv.CreateMat(height, width, sample_image.type)
cv.Resize(sample_image, resized_sample_image)
cv.Line(resized_sample_image, line1[0], line1[1], cv.CV_RGB(0, 255, 0))
cv.Line(resized_sample_image, line2[0], line2[1], cv.CV_RGB(0, 255, 0))
boundary_pts = line1 + line2
return resized_sample_image, \
cvutils.reorder_boundary_points(
np.array(full_line1 + full_line2))
def step(self, show):
r_image = cv.QueryFrame(self.capture)
if Camera.EXTERNAL:
self.image = cv.CreateImage((r_image.height, r_image.width), r_image.depth, r_image.channels)
cv.Transpose(r_image, self.image)
cv.Flip(self.image, self.image, flipMode=0)
else:
self.image = r_image
self.detect_faces()
for (x,y,w,h) in self.faces:
cv.PutText(self.image, "LOOK AT THIS IDIOT", (0, 30), Camera.FONT, Camera.RED)
cv.PutText(self.image, str(time.time()), (self.image.width - 275, self.image.height - 20), Camera.FONT, Camera.RED)
cv.Line(self.image, (0, y+h/2), (self.image.width, y+h/2), Camera.RED)
cv.Line(self.image, (x+w/2, 0), (x+w/2, self.image.height), Camera.RED)
cv.Circle(self.image, (x+w/2, y+h/2), min(h/2, w/2), Camera.RED, thickness=2)
cv.Circle(self.image, (x+w/2, y+h/2), min(h/8, w/8), Camera.RED)
#arrow(self.image, (200, 40), (x, y), Camera.WHITE)
if show:
print self.faces
cv.ShowImage("w1", self.image)
cv.WaitKey(1)
return self.image
def listen(self):
if isinstance(self.background, np.ndarray):
bgimg = cv.CreateImage(self.background.shape[:2], 8, 3)
img = cv.CreateImage(self.background.shape[:2], 8, 3)
theWidth = self.background.shape[1]
theHeight = self.background[0]
else:
bgimg = cv.CreateImage(
(self.background.width, self.background.height), 8, 3)
img = cv.CreateImage(
(self.background.width, self.background.height), 8, 3)
theWidth = self.background.width
theHeight = self.background.height
cv.Copy(self.background, bgimg)
smallimg = cv.CreateImage(
(theWidth / self.zoom, theHeight / self.zoom), 8, 3)
cv.GetRectSubPix(bgimg, smallimg,
(theWidth / (2 * self.zoom) + self.offset[0],
theHeight / (2 * self.zoom) + self.offset[1]))
cv.Resize(smallimg, img)
if (self.cp != False):
cv.Circle(img, self.zoomPt(self.cp.x, self.cp.y), 3,
cv.RGB(0, 255, 0), -1)
cv.Line(img, (self.ch_x - 25, self.ch_y), (self.ch_x + 25, self.ch_y),
cv.RGB(255, 255, 0))
cv.Line(img, (self.ch_x, self.ch_y - 25), (self.ch_x, self.ch_y + 25),
cv.RGB(255, 255, 0))
cv.ShowImage(self.name, img)
cv.WaitKey(25)
def draw_pizza(self):
print "Number of boxes:", len(self.Boxes)
for Box in self.Boxes:
line_color = (random.randint(0, 255), random.randint(0, 255),
random.randint(0, 255))
cv.Line(self.debug_frame, Box.corner1, Box.corner2, line_color, 10,
cv.CV_AA, 0)
cv.Line(self.debug_frame, Box.corner1, Box.corner3, line_color, 10,
cv.CV_AA, 0)
cv.Line(self.debug_frame, Box.corner3, Box.corner4, line_color, 10,
cv.CV_AA, 0)
cv.Line(self.debug_frame, Box.corner2, Box.corner4, line_color, 10,
cv.CV_AA, 0)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2, 1)
cv.PutText(self.debug_frame, str("1"),
(int(Box.corner1[0]), int(Box.corner1[1])), font,
(0, 0, 255))
cv.PutText(self.debug_frame, str("2"),
(int(Box.corner2[0]), int(Box.corner2[1])), font,
(0, 0, 255))
cv.PutText(self.debug_frame, str("3"),
(int(Box.corner3[0]), int(Box.corner3[1])), font,
(0, 0, 255))
cv.PutText(self.debug_frame, str("4"),
(int(Box.corner4[0]), int(Box.corner4[1])), font,
(0, 0, 255))
center = (int(Box.center[0]), int(Box.center[1]))
cv.Circle(self.debug_frame, center, 15, (255, 0, 0), 2, 8, 0)
def redraw():
global draging
global has_roi
global roi_x0
global roi_y0
global cur_mouse_x
global cur_mouse_y
#Redraw ROI selection
image2 = cv.CloneImage(current_image)
# redraw old rect
pen_width = 4
if rect_table.has_key(current_img_file_name):
rects_in_table = rect_table[current_img_file_name]
for r in rects_in_table:
cv.Rectangle(image2, (r[0], r[1]), (r[0] + r[2], r[1] + r[3]),
cv.CV_RGB(0, 255, 0), pen_width)
# redraw new rect
if has_roi:
cv.Rectangle(image2, (roi_x0, roi_y0), (cur_mouse_x, cur_mouse_y),
cv.CV_RGB(255, 0, 255), pen_width)
# draw background
if current_img_file_name in background_files:
cv.Line(image2, (0, 0), (image2.width, image2.height),
cv.CV_RGB(255, 0, 0))
cv.Line(image2, (0, image2.height), (image2.width, 0),
cv.CV_RGB(255, 0, 0))
cv.ShowImage(window_name, image2)
def draw_defects(self, image):
for d in self.defects:
start, end, depth = d
cv.Line(image, start, end, (0, 64, 128), 1, 8, 0)
cv.Line(image, end, depth, (0, 64, 128), 2, 8, 0)
if len(self.defects) >= 2 and self.center_defects:
cv.Circle(image, self.center_defects, 24, (255, 255, 0), 15, 7, 0)
def main():
""" main loop for displaying UI and processing key presses """
global NEW_COORDS
calibrateCoords() # calibrate scaling based on map coordinates
while rospy.is_shutdown() == False:
for key in NODE_NUMS:
coord = NODE_COORDS[key]
transf_coord = coordTransform(coord)
NEW_COORDS[key] = transf_coord
# draw hallway lines
for edge in LIBRA:
src_node = edge[0]
dst_node = edge[1]
src = NEW_COORDS[src_node]
dst = NEW_COORDS[dst_node]
cv.Line(D.image, src, dst, cv.RGB(0, 145, 255), thickness=2)
# draw node circles
for key in NODE_NUMS:
coord = NEW_COORDS[key]
state = D.node_states[key]
color = COLORS[state]
cv.Line(D.image, coord, coord, color, thickness=12)
# wait and check for quit request
key_code = cv.WaitKey(10) & 255
key_press = chr(key_code)
if key_code == 27 or key_press == 'q': # if ESC or 'q' was pressed
rospy.signal_shutdown("Quitting...")
cv.ShowImage("LibraComplex", D.image)
def drawpart(self, cv_image, T, KK):
if self.trimesh is not None:
vertices = self.trimesh.vertices
indices = self.trimesh.indices
elif self.boundingbox is not None:
vertices, indices = ComputeBoxMesh(
0.5 * (self.boundingbox[1, :] - self.boundingbox[0, :]))
vertices += tile(
0.5 * (self.boundingbox[1, :] + self.boundingbox[0, :]),
(len(vertices), 1))
else:
return
N = vertices.shape[0]
pts = dot(transformPoints(T, vertices), transpose(KK))
imagepts = pts[0:N, 0:2] / reshape(repeat(pts[0:N, 2], 2), [N, 2])
cvimagepts = [tuple(p) for p in array(imagepts, int)]
for tri in indices:
cv.Line(cv_image,
cvimagepts[tri[0]],
cvimagepts[tri[1]], (255, 255, 255),
thickness=1)
cv.Line(cv_image,
cvimagepts[tri[1]],
cvimagepts[tri[2]], (255, 255, 255),
thickness=1)
cv.Line(cv_image,
cvimagepts[tri[2]],
cvimagepts[tri[0]], (255, 255, 255),
thickness=1)
def addoverlay(self):
self.write(__DOC__, (10, 20))
self.write('orient.py', (10, self.size[1] - 10))
cv.Line(self.frame, (0, self.center[1]),
(self.size[0], self.center[1]), self.color)
cv.Line(self.frame, (self.center[0], 0),
(self.center[0], self.size[1]), self.color)
cv.Circle(self.frame, self.center, 100, self.color)
def drawcoordsys(self, cv_image, T, KK):
points3d = array(((0, 0, 0), (0.05, 0, 0), (0, 0.05, 0), (0, 0, 0.05)))
projpts = dot(transformPoints(T, points3d), transpose(KK))
x = array(projpts[:, 0] / projpts[:, 2], int)
y = array(projpts[:, 1] / projpts[:, 2], int)
cv.Line(cv_image, (x[0], y[0]), (x[1], y[1]), (0, 0, 255), thickness=3)
cv.Line(cv_image, (x[0], y[0]), (x[2], y[2]), (0, 255, 0), thickness=3)
cv.Line(cv_image, (x[0], y[0]), (x[3], y[3]), (255, 0, 0), thickness=3)
def drawpart(self,cv_image,T,KK):
N = self.trimesh.vertices.shape[0]
pts = dot(transformPoints(T,self.trimesh.vertices),transpose(KK))
imagepts = pts[0:N,0:2]/reshape(repeat(pts[0:N,2],2),[N,2])
cvimagepts = [tuple(p) for p in array(imagepts,int)]
for tri in self.trimesh.indices:
cv.Line(cv_image,cvimagepts[tri[0]],cvimagepts[tri[1]],(255,255,255))
cv.Line(cv_image,cvimagepts[tri[1]],cvimagepts[tri[2]],(255,255,255))
cv.Line(cv_image,cvimagepts[tri[2]],cvimagepts[tri[0]],(255,255,255))
def draw_lines(self, im, color=cv.RGB(255,255,255), thickness=5):
t1, t2 = (self.size[0]/3, self.size[1]/3)
b1, b2 = t1/3, t2/3
cv.Line(im, (t1, 0), (t1, self.size[1]), color, thickness=thickness)
cv.Line(im, (t1*2, 0), (t1*2, self.size[1]), color, thickness=thickness)
cv.Line(im, (0, t2), (self.size[0], t2), color, thickness=thickness)
cv.Line(im, (0, t2*2), (self.size[0], t2*2), color, thickness=thickness)
return im
def drawQuad(image, quads):
cv.Line(image, (quads[0][0], quads[0][1]), (quads[1][0], quads[1][1]),
cv.CV_RGB(255, 120, 0), 3, 4)
cv.Line(image, (quads[1][0], quads[1][1]), (quads[2][0], quads[2][1]),
cv.CV_RGB(255, 120, 0), 3, 4)
cv.Line(image, (quads[2][0], quads[2][1]), (quads[3][0], quads[3][1]),
cv.CV_RGB(255, 120, 0), 3, 4)
cv.Line(image, (quads[3][0], quads[3][1]), (quads[0][0], quads[0][1]),
cv.CV_RGB(255, 120, 0), 3, 4)
def sort_bins(self):
# promote candidate to confirmed if seen enough times, if it hasn't been seen, delete the bin
for candidate in self.candidates:
candidate.last_seen -= 1
if candidate.last_seen < self.last_seen_thresh:
self.candidates.remove(candidate)
print "lost"
continue
if candidate.seencount > self.min_seencount:
self.confirmed.append(candidate)
self.candidates.remove(candidate)
print "confirmed"
continue
self.min_perimeter = 500000
self.angles = []
for confirmed in self.confirmed:
if 0 < line_distance(confirmed.corner1, confirmed.corner3) * 2 + line_distance(confirmed.corner1, confirmed.corner2) * 2 < self.min_perimeter:
self.min_perimeter = line_distance(confirmed.corner1, confirmed.corner3) * 2 + line_distance(confirmed.corner1, confirmed.corner2) * 2
# print confirmed.angle/math.pi*180
self.angles.append(cv.Round(confirmed.angle / math.pi * 180 / 10) * 10)
# compare perimeter of existing bins. If a bin is too much bigger than the others, it is deleted. This is done to get rid of bins found based of 3 bins
for confirmed in self.confirmed:
if math.fabs(line_distance(confirmed.corner1, confirmed.corner3) * 2 + math.fabs(line_distance(confirmed.corner1, confirmed.corner2) * 2) - self.min_perimeter) > self.min_perimeter * self.perimeter_threshold and line_distance(confirmed.corner1, confirmed.corner3) * 2 + line_distance(confirmed.corner1, confirmed.corner2) * 2 > self.min_perimeter:
print "perimeter error (this is a good thing)"
print math.fabs(line_distance(confirmed.corner1, confirmed.corner3) * 2 + math.fabs(line_distance(confirmed.corner1, confirmed.corner2) * 2) - self.min_perimeter), "is greater than", self.min_perimeter * self.perimeter_threshold
print "yay?"
confirmed.last_seen -= 5
continue
confirmed.last_seen -= 1
if confirmed.last_seen < self.last_seen_thresh:
self.confirmed.remove(confirmed)
print "lost confirmed"
continue
# draw bins
line_color = (confirmed.corner1[1] / 2, confirmed.corner2[1] / 2, confirmed.corner4[1] / 2)
cv.Circle(self.debug_frame, (int(confirmed.midx), int(confirmed.midy)), 15, line_color, 2, 8, 0)
pt1 = (cv.Round(confirmed.corner1[0]), cv.Round(confirmed.corner1[1]))
pt2 = (cv.Round(confirmed.corner2[0]), cv.Round(confirmed.corner2[1]))
cv.Line(self.debug_frame, pt1, pt2, line_color, 1, cv.CV_AA, 0)
pt2 = (cv.Round(confirmed.corner2[0]), cv.Round(confirmed.corner2[1]))
pt4 = (cv.Round(confirmed.corner4[0]), cv.Round(confirmed.corner4[1]))
pt3 = (cv.Round(confirmed.corner3[0]), cv.Round(confirmed.corner3[1]))
cv.Line(self.debug_frame, pt2, pt4, line_color, 1, cv.CV_AA, 0)
cv.Line(self.debug_frame, pt3, pt4, line_color, 1, cv.CV_AA, 0)
cv.Line(self.debug_frame, pt1, pt3, line_color, 1, cv.CV_AA, 0)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, .6, .6, 0, 1, 1)
text_color = (0, 255, 0)
# print id and last_seen by each bin
cv.PutText(self.debug_frame, str(confirmed.id), (int(confirmed.midx), int(confirmed.midy)), font, confirmed.debug_color)
cv.PutText(self.debug_frame, str(confirmed.last_seen), (int(confirmed.midx - 20), int(confirmed.midy - 20)), font, confirmed.debug_color)
def printLine(img, length):
px = img.width / 2
py = img.height / 2
minx = max(px - length, 0)
miny = max(py - length, 0)
maxx = min(px + length, img.width)
maxy = min(py + length, img.height)
cv.Line(img, (px, miny), (px, maxy), cv.CV_RGB(0, 0, 0))
cv.Line(img, (minx, py), (maxx, py), cv.CV_RGB(255, 255, 255))
def __init__(self):
grid_spacing = rospy.get_param('~grid_spacing')
self.bridge = CvBridge()
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('projector/get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'projector/get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('projector/set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('projector/set_projection',
projector.srv.SetProjection)
projector_info = projector_info_service().projector_info
projector_model = image_geometry.PinholeCameraModel()
projector_model.fromCameraInfo(projector_info)
# Generate grid projections
self.original_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
for row in range(0, projector_info.height, grid_spacing):
cv.Line(self.original_projection, (0, row),
(projector_info.width - 1, row), 255)
for col in range(0, projector_info.width, grid_spacing):
cv.Line(self.original_projection, (col, 0),
(col, projector_info.height - 1), 255)
predistortmap_x = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
InitPredistortMap(projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs(), predistortmap_x,
predistortmap_y)
self.predistorted_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
cv.Remap(self.original_projection,
self.predistorted_projection,
predistortmap_x,
predistortmap_y,
flags=cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS,
fillval=(0, 0, 0, 0))
self.off_projection = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_8UC1)
cv.SetZero(self.off_projection)
def test_diebold():
testdir = 'diebold_test'
# 0.) Gather test data
test_data = {} # maps {str imgpath: (int orient, str decoding)}
for dirpath, dirnames, filenames in os.walk(testdir):
for imgname in [f for f in filenames if is_img_ext(f)]:
orient = VERTICAL if 'vert' in imgname else HORIZONTAL
imgpath = os.path.join(dirpath, imgname)
foo = os.path.splitext(imgname)[0].split(
'_')[0] if orient == VERTICAL else os.path.splitext(imgname)[0]
resname = foo + '.res'
respath = os.path.join(dirpath, resname)
decoding = open(respath, 'r').readlines()[0].strip()
test_data[imgpath] = (orient, decoding)
markpath = 'diebold_mark_v2.png'
mark = cv.LoadImage(markpath, cv.CV_LOAD_IMAGE_GRAYSCALE)
w, h = cv.GetSize(mark)
mark_rot = cv.CreateImage((h, w), mark.depth, mark.channels)
cv.Transpose(mark, mark_rot)
for i, (imgpath, (orient, decoding)) in enumerate(test_data.iteritems()):
I = cv.LoadImage(imgpath, cv.CV_LOAD_IMAGE_GRAYSCALE)
syms = parse_patch(
I,
cv.GetSize(mark) if orient == HORIZONTAL else cv.GetSize(mark_rot),
orient=orient)
decoding_guess = ''.join(t[0] for t in syms)
print decoding == decoding_guess
print " {0}".format(imgpath)
# Draw marks
w_mark, h_mark = cv.GetSize(
mark) if orient == HORIZONTAL else cv.GetSize(mark_rot)
Icolor = cv.LoadImage(imgpath, cv.CV_LOAD_IMAGE_COLOR)
for idx, (sym, x1) in enumerate(syms):
if orient == HORIZONTAL:
cv.Line(Icolor, (x1, 0), (x1, h_mark - 1),
cv.CV_RGB(230, 0, 0))
cv.Line(Icolor, (x1 + w_mark, 0), (x1 + w_mark, h_mark - 1),
cv.CV_RGB(0, 0, 230))
else:
y1 = x1
cv.Line(Icolor, (0, y1), (w_mark - 1, y1),
cv.CV_RGB(230, 0, 0))
cv.Line(Icolor, (0, y1 + h_mark - 1),
(w_mark - 1, y1 + h_mark - 1), cv.CV_RGB(0, 0, 230))
cv.SaveImage("_Icolor_res_{0}.png".format(i), Icolor)
return True
def arrow(img, p1, p2, color):
denom = p2[0]-p1[0]
angle = 3*math.pi/2
if not denom == 0:
angle = math.atan(float(p2[1]-p1[1])/(p2[0]-p1[0]))
p2a = (p2[0]-int(10*math.cos(angle+math.pi/4)),
p2[1]-int(10*math.sin(angle+math.pi/4)))
p2b = (p2[0]-int(10*math.cos(angle-math.pi/4)),
p2[1]-int(10*math.sin(angle-math.pi/4)))
cv.Line(img, p1, p2, color, 2)
cv.Line(img, p2a, p2, color, 2)
cv.Line(img, p2b, p2, color, 2)
def Arrow(img, head, tail, color, thickness=1, lineType=0):
# draw the shaft
cv.Line(img, head, tail, color, thickness, lineType)
# determine head coordinates
a = atan2(head[1] - tail[1], head[0] - tail[0])
h = hypot(head[0] - tail[0], head[1] - tail[1])
t1 = (int(head[0] - h / 3 * cos(a + pi / 6)),
int(head[1] - h / 3 * sin(a + pi / 6)))
t2 = (int(head[0] - h / 3 * cos(a - pi / 6)),
int(head[1] - h / 3 * sin(a - pi / 6)))
cv.Line(img, head, t1, color, thickness, lineType)
cv.Line(img, head, t2, color, thickness, lineType)
def drawFeetPoint(self,img,imgCoords,RWFeet): """ Draw a point as a red cross """ typeImg=type(img) if typeImg==np.ndarray: cv2.line(img, (imgCoords[0]-5,imgCoords[1]), (imgCoords[0]+5,imgCoords[1]), cv.CV_RGB(255,0,0), thickness=1, lineType=8, shift=0) cv2.line(img, (imgCoords[0],imgCoords[1]-5), (imgCoords[0],imgCoords[1]+5), cv.CV_RGB(255,0,0), thickness=1, lineType=8, shift=0) #text="X:"+str(RWFeet[0])+"m, Y:"+str(RWFeet[1])+"m" text="X:"+"%0.2f" % (RWFeet[0],)+"m, Y:"+"%0.2f" % (RWFeet[1],)+"m" cv2.putText(img, text, (imgCoords[0]-len(text)*4,imgCoords[1]+12), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), thickness = 1, lineType=cv2.CV_AA) else: cv.Line(img, (imgCoords[0]-5,imgCoords[1]), (imgCoords[0]+5,imgCoords[1]), cv.CV_RGB(255,0,0), thickness=1, lineType=8, shift=0) cv.Line(img, (imgCoords[0],imgCoords[1]-5), (imgCoords[0],imgCoords[1]+5), cv.CV_RGB(255,0,0), thickness=1, lineType=8, shift=0) cv.PutText(img, "X:"+str(RWFeet[0])+"m, Y:"+str(RWFeet[1])+"m", (imgCoords[0]-len(str(RWFeet)*8),imgCoords[1]+8), cv.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 0), thickness = 2, linetype=cv2.CV_AA)
def draw_gui(cv, img):
if not 'w' in gui: gui['w'] = img.width
if not 'h' in gui: gui['h'] = img.height
if not 'frame' in gui:
frame = {
'w': int(img.width / 2 + .5),
'h': int(img.height / 2 + .5),
'x': int(img.width / 4 + .5),
'y': int(img.height / 4 + .5),
'c': (255, 0, 255)
}
gui['frame'] = frame
f = gui['frame']
ww = int(f['w'] / 8 + .5)
hh = int(f['h'] / 8 + .5)
col = f['c']
cv.Line(img, (f['x'], f['y']), (f['x'] + ww, f['y']), col, 1)
cv.Line(img, (f['x'], f['y']), (f['x'], f['y'] + hh), col, 1)
cv.Line(img, (f['x'], f['y'] + f['h']), (f['x'] + ww, f['y'] + f['h']),
col, 1)
cv.Line(img, (f['x'], f['y'] + f['h']), (f['x'], f['y'] + f['h'] - hh),
col, 1)
cv.Line(img, (f['x'] + f['w'], f['y'] + f['h']),
(f['x'] + f['w'] - ww, f['y'] + f['h']), col, 1)
cv.Line(img, (f['x'] + f['w'], f['y'] + f['h']),
(f['x'] + f['w'], f['y'] + f['h'] - hh), col, 1)
cv.Line(img, (f['x'] + f['w'], f['y']), (f['x'] + f['w'] - ww, f['y']),
col, 1)
cv.Line(img, (f['x'] + f['w'], f['y']), (f['x'] + f['w'], f['y'] + hh),
col, 1)
def wylicz_dynamiczna_styczna_do_prazka(self,
prazek,
punkty_na_styczna,
quant_x0,
test_freq,
gray,
gora_czy_dol=1):
konce_stycznej = []
odcinki = []
j = 0
line_ends = [[0, 0], [0, 0]] # odcinek styczny miesjcowo do prazka
ilosc_odcinkow = len(prazek) / punkty_na_styczna
height = cv.GetSize(gray)[1]
if gora_czy_dol == -1: # dla dolnych prazkow
for i in range(ilosc_odcinkow):
if i == 0:
line_ends[0][0] = prazek[i * punkty_na_styczna]
line_ends[0][1] = height - (i * punkty_na_styczna +
quant_x0) * test_freq
else:
line_ends[1][0] = prazek[i * punkty_na_styczna]
line_ends[1][1] = height - (i * punkty_na_styczna +
quant_x0) * test_freq
odcinki.append(line_ends)
cv.Line(gray, (line_ends[0][0], line_ends[0][1]),
(line_ends[1][0], line_ends[1][1]), (210, 0, 0), 2)
line_ends = [[0, 0], [0, 0]]
line_ends[0][0] = prazek[i * punkty_na_styczna]
line_ends[0][1] = height - (i * punkty_na_styczna +
quant_x0) * test_freq
else: # dla gornych prazkow
for i in range(ilosc_odcinkow):
if i == 0:
line_ends[0][0] = prazek[i * punkty_na_styczna]
line_ends[0][1] = (i * punkty_na_styczna +
quant_x0) * test_freq
else:
line_ends[1][0] = prazek[i * punkty_na_styczna]
line_ends[1][1] = (i * punkty_na_styczna +
quant_x0) * test_freq
odcinki.append(line_ends)
cv.Line(gray, (line_ends[0][0], line_ends[0][1]),
(line_ends[1][0], line_ends[1][1]), (210, 0, 0), 2)
line_ends = [[0, 0], [0, 0]]
line_ends[0][0] = prazek[i * punkty_na_styczna]
line_ends[0][1] = (i * punkty_na_styczna +
quant_x0) * test_freq
return odcinki
def draw(self, img):
'''
Draw this corner on img
@param img:
'''
rect = cv.GetImageROI(img)
cv.ResetImageROI(img)
col = (255, 255, 255)
cv.Line(img, self.prev, self.p, col)
cv.Line(img, self.p, self.next, col)
# cv.Line(img, self.p, add(self.p, rotateVec((30, 0), self.rotation), 1), col)
cv.Circle(img, self.p, 2, col)
font = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX_SMALL, 0.7, 0.7)
cv.PutText(img, "(%d,%d)" % self.p, self.p, font, col)
cv.SetImageROI(img, rect)