def ccoeff_normed(img1, img2): size = cv.GetSize(img1) tmp1 = float_version(img1) tmp2 = float_version(img2) cv.SubS(tmp1, cv.Avg(tmp1), tmp1) cv.SubS(tmp2, cv.Avg(tmp2), tmp2) norm1 = cv.CloneImage(tmp1) norm2 = cv.CloneImage(tmp2) cv.Pow(tmp1, norm1, 2.0) cv.Pow(tmp2, norm2, 2.0) #cv.Mul(tmp1, tmp2, tmp1) return cv.DotProduct(tmp1, tmp2) / (cv.Sum(norm1)[0]*cv.Sum(norm2)[0])**0.5
def normalize_plane(plane, aggressive=0):
if aggressive:
# smooth = image_empty_clone(plane)
# cv.Smooth(plane, smooth, cv.CV_GAUSSIAN, 13, 13)
hist = get_gray_histogram(plane, bins=255)
_, max_value, _, max_color = cv.GetMinMaxHistValue(hist)
thr_value = max_value * aggressive
down_threshold, up_threshold = None, None
for k in range(256):
down_val = cv.QueryHistValue_1D(hist, k)
up_val = cv.QueryHistValue_1D(hist, 254 - k)
if down_threshold is None and down_val >= thr_value:
down_threshold = k
if up_threshold is None and up_val >= thr_value:
up_threshold = k
if down_threshold is not None and up_threshold is not None:
break
sub_plane = None
if down_threshold > 0:
sub_plane = image_empty_clone(plane)
cv.SubS(plane, down_threshold, sub_plane)
add_plane = None
if down_threshold + up_threshold > 0:
add_plane = image_empty_clone(plane)
cv.AddS(sub_plane or plane, down_threshold + up_threshold,
add_plane)
plane = add_plane or plane
norm_plane = image_empty_clone(plane)
cv.Normalize(plane, norm_plane, 0, 255, cv.CV_MINMAX)
return norm_plane
def test2():
path = "/Users/soswow/Documents/Face Detection/gray_test"
for file_path, name in directory_files(path):
if name.endswith(".jpg"):
img = cv.LoadImage(file_path, iscolor=False)
N = sizeOf(img)[0] * sizeOf(img)[1]
I_bar = cv.Sum(img)[0] / N
tmp = image_empty_clone(img)
cv.SubS(img, I_bar, tmp)
tmp2 = image_empty_clone(img)
cv.AddS(tmp, 128, tmp2)
merged = merge_images(img, tmp2, vertical=True)
cv.SaveImage(join(path, "_%s" % name), merged)
def image_processor():
cv.Smooth(gray_image, gray_image, cv.CV_GAUSSIAN, 3,
3) #Blurring to remove some noise
cv.AbsDiff(prev_image, gray_image,
accumulator) #Getting the difference image
cv.InRangeS(accumulator, threshold_limit1_lower, threshold_limit1_upper,
accumulator) #Thresholding the difference image
cv.Dilate(accumulator, accumulator, None,
2) #Dilating the thresholded difference image
cv.Add(accumulator, sum_image, sum_image,
accumulator) #Adding the image to a register to use fading
cv.SubS(sum_image, fading_factor, sum_image) #Fading
cv.InRangeS(sum_image, threshold_limit2_lower, threshold_limit2_upper,
accumulator) #Thresholding the fading image
cv.Copy(gray_image, prev_image)
cv.Copy(accumulator, temp_image)
def process_frame(self, frame):
found_path = False
cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)
# use RGB color finder
binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(
frame, self.R, self.G, self.B, self.min_blob_size, self.dev_thresh,
self.precision / 1000.0)
if self.debug:
color_filtered = cv.CloneImage(binary)
svr.debug("color_filtered", color_filtered)
# Get Edges
cv.Canny(binary, binary, 30, 40)
# Hough Transform
line_storage = cv.CreateMemStorage()
lines = cv.HoughLines2(binary,
line_storage,
cv.CV_HOUGH_STANDARD,
rho=1,
theta=math.pi / 180,
threshold=self.hough_threshold,
param1=0,
param2=0)
lines = lines[:self.lines_to_consider] # Limit number of lines
# If there are at least 2 lines and they are close to parallel...
# There's a path!
#print lines
if len(lines) >= 2:
# Find: min, max, average
theta_max = lines[0][1]
theta_min = lines[0][1]
total_theta = 0
for rho, theta in lines:
total_theta += theta
if theta_max < theta:
theta_max = theta
if theta_min > theta:
theta_min = theta
theta_range = theta_max - theta_min
# Near vertical angles will wrap around from pi to 0. If the range
# crosses this vertical line, the range will be way too large. To
# correct for this, we always take the smallest angle between the
# min and max.
if theta_range > math.pi / 2:
theta_range = math.pi - theta_range
if theta_range < self.theta_threshold:
found_path = True
angles = map(lambda line: line[1], lines)
self.theta = circular_average(angles, math.pi)
print found_path
if found_path:
self.seen_in_a_row += 1
else:
self.seen_in_a_row = 0
# stores whether or not we are confident about the path's presence
object_present = False
print self.seen_in_a_row ###
if self.seen_in_a_row >= self.seen_in_a_row_threshold:
object_present = True
self.image_coordinate_center = self.find_centroid(binary)
# Move the origin to the center of the image
self.center = (self.image_coordinate_center[0] - frame.width / 2,
self.image_coordinate_center[1] * -1 +
frame.height / 2)
if self.debug:
# Show color filtered
color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
cv.Sub(frame, color_filtered_rgb, frame)
# Show edges
binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
cv.Add(frame, binary_rgb, frame) # Add white to edge pixels
cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
cv.Sub(frame, binary_rgb, frame) # Remove all but Red
# Show lines
if self.seen_in_a_row >= self.seen_in_a_row_threshold:
rounded_center = (
int(round(self.image_coordinate_center[0])),
int(round(self.image_coordinate_center[1])),
)
cv.Circle(frame, rounded_center, 5, (0, 255, 0))
libvision.misc.draw_lines(frame,
[(frame.width / 2, self.theta)])
else:
libvision.misc.draw_lines(frame, lines)
#cv.ShowImage("Path", frame)
svr.debug("Path", frame)
# populate self.output with infos
self.output.found = object_present
self.output.theta = self.theta
if self.center:
# scale center coordinates of path based on frame size
self.output.x = self.center[0] / (frame.width / 2)
self.output.y = self.center[1] / (frame.height / 2)
libvision.misc.draw_linesC(frame,
[(frame.width / 2, self.output.theta)],
[255, 0, 255])
print "Output Returned!!! ", self.output.theta
else:
self.output.x = None
self.output.y = None
print "No output..."
if self.output.found and self.center:
print self.output
self.return_output()
def process_frame(self, frame):
if self.path_manager.start_angle is None:
self.path_manager.start_angle = get_yaw()
self.output.found = False
cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)
# Use RGB color finder
binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(
frame, 250, 125, 0, 1500, 500, .3)
color_filtered = cv.CloneImage(binary)
blob_map = cv.CloneImage(binary)
blobs = libvision.blob.find_blobs(binary,
blob_map,
min_blob_size=50,
max_blobs=10)
if not blobs:
return
binary = cv.CloneImage(blob_map)
mapping = [0] * 256
for blob in blobs:
mapping[blob.id] = 255
libvision.greymap.greymap(blob_map, binary, mapping)
# Get Edges
cv.Canny(binary, binary, 30, 40)
# Hough Transform
line_storage = cv.CreateMemStorage()
lines = cv.HoughLines2(binary,
line_storage,
cv.CV_HOUGH_STANDARD,
rho=1,
theta=math.pi / 180,
threshold=self.hough_threshold,
param1=0,
param2=0)
lines = lines[:self.lines_to_consider] # Limit number of lines
if not lines:
return
paths = self.path_manager.process(lines, blobs)
if paths and not self.path:
# If path[1] is clockwise of paths[0]
distance = circular_distance(paths[0].angle, paths[1].angle)
print
print "Distance: ", distance
print paths[0].theta, paths[0].angle
print paths[1].theta, paths[1].angle
if distance > 0:
self.path = paths[self.which_path]
else:
self.path = paths[1 - self.which_path]
print self.path.angle, self.path.theta
print
if paths and self.path in paths and self.path.blobs:
temp_map = cv.CloneImage(blob_map)
mapping = [0] * 256
for blob in self.path.blobs:
mapping[blob.id] = 255
libvision.greymap.greymap(blob_map, temp_map, mapping)
center = self.find_centroid(temp_map)
svr.debug("map", temp_map)
self.path.center = (center[0] - (frame.width / 2),
-center[1] + (frame.height / 2))
self.output.found = True
self.output.theta = self.path.theta
self.output.x = self.path.center[0] / (frame.width / 2)
self.output.y = self.path.center[1] / (frame.height / 2)
print self.output
if self.debug:
# Show color filtered
color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
cv.Sub(frame, color_filtered_rgb, frame)
# Show edges
binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
cv.Add(frame, binary_rgb, frame) # Add white to edge pixels
cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
cv.Sub(frame, binary_rgb, frame) # Remove all but Red
theta = math.radians(
circular_distance(self.path_manager.start_angle, get_yaw()))
if theta < 0:
scale = math.cos(-2 * theta)
theta = pi + theta
libvision.misc.draw_lines(
frame, [((-frame.width / 2) * scale, theta)])
else:
libvision.misc.draw_lines(frame, [(frame.width / 2, theta)])
# Show lines
if self.output.found:
rounded_center = (
int(round(center[0])),
int(round(center[1])),
)
cv.Circle(frame, rounded_center, 5, (0, 255, 0))
libvision.misc.draw_lines(frame,
[(frame.width / 2, self.path.theta)])
else:
libvision.misc.draw_lines(frame, lines)
svr.debug("Path", frame)
self.return_output()
#Acquiring the image
img = cv.QueryFrame(capture)
#Showing image
if flag_true_image:
cv.ShowImage(window1, gray_image)
else:
cv.ShowImage(window1, render_image)
#Image processing
cv.CvtColor(img, gray_image, cv.CV_RGB2GRAY)
cv.Copy(gray_image, register1_image)
cv.Smooth(register1_image, register1_image, cv.CV_GAUSSIAN, 3, 3)
cv.AbsDiff(register1_image, register2_image, accumulator)
cv.InRangeS(accumulator, (threshold_limit1_lower), (threshold_limit1_upper), accumulator)
cv.Dilate(accumulator, accumulator, None, 2)
cv.Add(accumulator, sum_image, sum_image, accumulator)
cv.SubS(sum_image, (fading_factor), sum_image)
cv.InRangeS(sum_image, (threshold_limit2_lower), (threshold_limit2_upper), accumulator)
cv.Copy(register1_image, register2_image)
#Motion detection
new_corners, status, track_error = cv.CalcOpticalFlowPyrLK(prev_image, gray_image, pyramid1, pyramid2, corners, (10,10), 2, (cv.CV_TERMCRIT_ITER, 10, 0), 0)
counter = (counter + 1) % skip
if(counter == 0):
corners = cv.GoodFeaturesToTrack(gray_image, eigen_image, temp_image, cornerCount = corner_count, qualityLevel = quality, minDistance = min_distance) #Good features to track
flag = True
cv.Copy(img, render_image)
cv.CvtColor(accumulator, render_image, cv.CV_GRAY2RGB)
#cv.Copy(img, render_image)
cv.Copy(gray_image, prev_image)
#Drawing vectors and averaging the rotation...
sum = 0
summing_counter = 0
def camera():
found_goals = False
print "# Starting initialization..."
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.Zero(intrinsics)
#camera data
intrinsics[0, 0] = 850.850708957251072
intrinsics[1, 1] = 778.955239997982062
intrinsics[2, 2] = 1
intrinsics[0, 2] = 320.898495232253822
intrinsics[1, 2] = 380.213734835526282
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = -0.226795877008420
dist_coeffs[0, 1] = 0.139445565548056
dist_coeffs[0, 2] = 0.001245710462327
dist_coeffs[0, 3] = -0.001396618726445
print "# intrinsics loaded!"
#prepare memory
capture = cv.CaptureFromCAM(0)
src = cv.QueryFrame(capture)
size = GetSize(src)
dst0 = cv.CreateImage(size, src.depth, src.nChannels)
image_ROI = (0, 60, 640, 340)
size = (640, 340)
hue = cv.CreateImage(size, 8, 1)
sat = cv.CreateImage(size, 8, 1)
val = cv.CreateImage(size, 8, 1)
ball = cv.CreateImage(size, 8, 1)
yellow = cv.CreateImage(size, 8, 1)
blue = cv.CreateImage(size, 8, 1)
Set2D(hue, 4, 4, 255)
Set2D(sat, 4, 4, 255)
Set2D(val, 4, 4, 255)
Set2D(ball, 4, 4, 255)
Set2D(yellow, 4, 4, 255)
Set2D(blue, 4, 4, 255)
ballx = 0
bally = 0
print "# base images created..."
#####------------------adjustment data---------------------###############
#shadow
high = 40
low = 300
#threshold
thresred = 160
thresgreen = 220
thresblue = 254
#dilate
ex = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_RECT)
ex2 = cv.CreateStructuringElementEx(2, 2, 1, 1, cv.CV_SHAPE_RECT)
ex5 = cv.CreateStructuringElementEx(5, 5, 1, 1, cv.CV_SHAPE_RECT)
tHack = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_CROSS)
#ball
ballcount = 15
ballmaxarea = 200
ballminiarea = 45
ballcompact = 1.3
#blue
bluecount = 30
bluemaxarea = 1500
blueminiarea = 50
bluemaxdepth = 10
blueminidepth = 2
#yellow
yellowcount = 30
yellowmaxarea = 1000
yellowminiarea = 50
yellowmaxdepth = 10
yellowminidepth = 3.2
#####----------------------------------------
aa = time.time()
storage = cv.CreateMemStorage()
first = True
pitch = 0 # 0 for main pitch, 1 for alt pitch
countf = 0
print "# starting capture..."
print ''
capture = cv.CaptureFromCAM(0)
while (True):
src = cv.QueryFrame(capture)
#ShowImage('src',src)
cv.SetImageROI(dst0, (0, 0, 640, 480))
average = cv.CreateImage(size, 8, 3)
#barrel undistortion
cv.Undistort2(src, dst0, intrinsics, dist_coeffs)
#ROI = Region of Interests, crop the image
cv.SetImageROI(dst0, image_ROI)
dst = GetImage(dst0)
dst2 = cv.CreateImage(size, 8, 3)
Set2D(dst2, 4, 4, 255)
hsv = cv.CreateImage(size, 8, 3)
CvtColor(dst, hsv, CV_RGB2HSV)
cv.Split(hsv, hue, sat, val, None)
if (first):
#hist = cv.CreateHist([32,64], CV_HIST_ARRAY, [[0,180], [0,256]], 1)
#cv.CalcHist([hue, sat], hist, 0, None)
#values = cv.GetMinMaxHistValue(hist)
#print values
#tweak = values[3][0]
#if tweak >= 12:
# pitch = 1
#print ">>> tweak=",tweak,"pitch selected =",pitch
pitch = pitchSet
if pitch == 1:
base = cv.LoadImage("base.jpg", cv.CV_LOAD_IMAGE_UNCHANGED)
baseInv = cv.CreateImage(size, 8, 1)
cv.Not(base, baseInv)
#huecorr = cv.LoadImage("huecorr.jpg",cv.CV_LOAD_IMAGE_UNCHANGED)
#cv.Smooth(huecorr,huecorr)
#ShowImage("base",base)
#base = cv.CreateImage(size,8,1)
#base = GetImage(val)
#cv.Threshold(hue,hue,75,255,cv.CV_THRESH_BINARY_INV)
#cv.SaveImage("huecorr.jpg", hue)
#cv.Threshold(base,base,110,255,cv.CV_THRESH_BINARY)
#cv.SaveImage("base.jpg", base)
#cv.WaitKey(-1)
first = False
if (debug):
ShowImage("hue", hue)
ShowImage("sat", sat)
ShowImage("val", val)
if pitch == 1:
walls = cv.CreateImage(size, 8, 1)
cv.Threshold(val, walls, 50, 255, cv.CV_THRESH_BINARY_INV)
Set2D(walls, 4, 4, 255)
# BALL
# fixed this cause with another robot it was finding the ball on it. seems to work
Add(sat, hue, ball)
Sub(ball, walls, ball)
cv.SubS(ball, 60, ball, baseInv)
cv.Threshold(ball, ball, 170, 255, cv.CV_THRESH_BINARY)
cv.Erode(ball, ball, ex, 1)
cv.Dilate(ball, ball, ex2, 1)
Set2D(ball, 4, 4, 255)
# YELLOW
# cv.Threshold(hue,yellow,80,255,cv.CV_THRESH_BINARY)
cv.Threshold(val, yellow, 250, 255, cv.CV_THRESH_BINARY)
Sub(yellow, walls, yellow)
cv.Erode(yellow, yellow, ex, 1)
Set2D(yellow, 4, 4, 255)
# blue
cv.Add(walls, hue, blue)
cv.Threshold(blue, blue, 40, 255, cv.CV_THRESH_BINARY_INV)
cv.Erode(blue, blue, ex2, 2)
Set2D(blue, 4, 4, 255)
cv.Dilate(blue, blue, tHack, 2)
if pitch == 0:
ballcompact = 2.0
walls = cv.CreateImage(size, 8, 1)
cv.Threshold(val, walls, 50, 255, cv.CV_THRESH_BINARY_INV)
Set2D(walls, 4, 4, 255)
# BALL
#cv.Add(sat,val,ball)
#ShowImage("rawB",ball)
cv.Threshold(hue, ball, 110, 255, cv.CV_THRESH_BINARY)
cv.Erode(ball, ball, ex2, 1)
cv.Dilate(ball, ball, ex, 1)
# YELLOW
cv.Threshold(val, yellow, 240, 255, cv.CV_THRESH_BINARY)
# cv.Threshold(hue,yellow,80,255,cv.CV_THRESH_TOZERO)
# cv.Threshold(yellow,yellow,105,255,cv.CV_THRESH_TOZERO_INV)
# cv.Threshold(yellow,yellow,50,255,cv.CV_THRESH_BINARY)
cv.Erode(yellow, yellow, ex, 1)
cv.Dilate(yellow, yellow, tHack, 1)
# BLUE
CvtColor(dst, hsv, CV_BGR2HSV)
cv.Split(hsv, hue, sat, val, None)
cv.Threshold(hue, blue, 80, 255, cv.CV_THRESH_BINARY)
cv.Threshold(val, val, 80, 255, cv.CV_THRESH_BINARY_INV)
# Removes the walls
Sub(blue, val, blue)
Sub(yellow, val, yellow)
Sub(ball, val, ball)
cv.Erode(blue, blue, ex, 1)
Set2D(ball, 4, 4, 255)
Set2D(yellow, 4, 4, 255)
Set2D(blue, 4, 4, 255)
if (debug):
ShowImage("blue", blue)
ShowImage("yellow", yellow)
ShowImage("ball", ball)
#find ball
#seq = None
seq = cv.FindContours(ball, storage, cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
#print seq
while seq != None:
compact = 0
count = count + 1
if (count > ballcount):
break
#removed and pitch==0 no idea why it was there
if (cv.ContourArea(seq) != 0):
compact = ArcLength(seq) * ArcLength(seq) / (
4 * cv.ContourArea(seq) * math.pi)
if compact >= ballcompact:
print ">> compact: ", compact, ballcompact
seq = seq.h_next()
continue
area = cv.ContourArea(seq)
if (area == 0 or area > ballmaxarea
or area < ballminiarea): # or compact > ballcompact):
print ">> area: ", area, ballmaxarea, ballminiarea
seq = seq.h_next()
continue
else:
ballx = 0
bally = 0
for p in seq:
ballx = ballx + p[0]
bally = bally + p[1]
ballx = int(float(ballx) / len(seq))
bally = int(float(bally) / len(seq))
# print "compact=%f,area=%f" %(compact,area)
cv.Circle(dst, (ballx, bally), 4, cv.CV_RGB(255, 255, 255),
2, 8, 0)
cv.Circle(dst2, (ballx, bally), 4,
cv.CV_RGB(255, 255, 255), 2, 8, 0)
break
if (count > 15 or seq == None):
ballx = -1
bally = -1
print "# error: ball not found "
#find blue
seq = None
seq = cv.FindContours(blue, storage, cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while seq != None:
count = count + 1
if (count > bluecount):
break
if (cv.ContourArea(seq) < blueminiarea
or cv.ContourArea(seq) > bluemaxarea):
seq = seq.h_next()
continue
else:
hull = None
convex = None
#
hull = cv.ConvexHull2(seq, storage)
convex = cv.ConvexityDefects(seq, hull, storage)
if (len(convex) > 1):
convex = sorted(convex,
key=lambda (k1, k2, k3, k4): k4
) #sort by depth of the convex defect
if (convex[len(convex) - 1][3] < blueminidepth
or convex[len(convex) - 2][3] < blueminidepth
or convex[len(convex) - 1][3] > bluemaxdepth
or convex[len(convex) - 2][3] > bluemaxdepth):
cv.Line(dst, convex[len(convex) - 1][0],
convex[len(convex) - 1][2],
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, convex[len(convex) - 1][2],
convex[len(convex) - 1][1],
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst, convex[len(convex) - 2][0],
convex[len(convex) - 2][2],
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, convex[len(convex) - 2][2],
convex[len(convex) - 2][1],
cv.CV_RGB(0, 255, 255), 2, 8, 0)
seq = seq.h_next()
continue
else:
#find the T
blue_start1 = convex[len(convex) - 1][0]
blue_end1 = convex[len(convex) - 1][1]
blue_depth1 = convex[len(convex) - 1][2]
#draw the side line of T
cv.Line(dst, blue_start1, blue_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, blue_depth1, blue_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, blue_start1, blue_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, blue_depth1, blue_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
blue_start2 = convex[len(convex) - 2][0]
blue_end2 = convex[len(convex) - 2][1]
blue_depth2 = convex[len(convex) - 2][2]
cv.Line(dst, blue_start2, blue_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, blue_depth2, blue_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, blue_start2, blue_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, blue_depth2, blue_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
blue_from = ((blue_depth1[0] + blue_depth2[0]) / 2,
(blue_depth1[1] + blue_depth2[1]) / 2
) #calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(blue_start1[0] - blue_end2[0],
blue_start1[1] -
blue_end2[1]) > math.hypot(
blue_end1[0] - blue_start2[0],
blue_end1[1] - blue_start2[1]):
blue_to = ((blue_end1[0] + blue_start2[0]) / 2,
(blue_end1[1] + blue_start2[1]) / 2)
else:
blue_to = ((blue_start1[0] + blue_end2[0]) / 2,
(blue_start1[1] + blue_end2[1]) / 2)
cv.Line(dst, blue_from, blue_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst, blue_from, 1, cv.CV_RGB(255, 0, 0),
2, 8, 0)
cv.Circle(dst, blue_to, 1, cv.CV_RGB(0, 0, 0), 2,
8, 0)
cv.Line(dst2, blue_from, blue_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst2, blue_from, 1, cv.CV_RGB(255, 0, 0),
2, 8, 0)
cv.Circle(dst2, blue_to, 1,
cv.CV_RGB(255, 255, 255), 2, 8, 0)
break
else:
seq = seq.h_next()
continue
if (count > bluecount or seq == None):
blue_from = (0, 0)
blue_to = (0, 0)
print "# error: blue not found "
#find yellow
seq = None
seq = cv.FindContours(yellow, storage, cv.CV_RETR_LIST,
cv.CV_LINK_RUNS)
if seq != None:
count = 0
while seq != None:
count = count + 1
if (count > yellowcount):
break
area = cv.ContourArea(seq)
if (area < yellowminiarea or area > yellowmaxarea):
seq = seq.h_next()
continue
else:
hull = None
convex = None
#
hull = cv.ConvexHull2(seq, storage)
convex = cv.ConvexityDefects(seq, hull, storage)
if (len(convex) > 1):
convex = sorted(convex,
key=lambda (k1, k2, k3, k4): k4
) #sort by depth of the convex defect
if (convex[len(convex) - 1][3] < yellowminidepth
or convex[len(convex) - 2][3] < yellowminidepth
or convex[len(convex) - 1][3] > yellowmaxdepth
or
convex[len(convex) - 2][3] > yellowmaxdepth):
seq = seq.h_next()
continue
else:
#find the T
yellow_start1 = convex[len(convex) - 1][0]
yellow_end1 = convex[len(convex) - 1][1]
yellow_depth1 = convex[len(convex) - 1][2]
#draw the side line of T
cv.Line(dst, yellow_start1, yellow_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, yellow_depth1, yellow_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, yellow_start1, yellow_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, yellow_depth1, yellow_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
yellow_start2 = convex[len(convex) - 2][0]
yellow_end2 = convex[len(convex) - 2][1]
yellow_depth2 = convex[len(convex) - 2][2]
cv.Line(dst, yellow_start2, yellow_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, yellow_depth2, yellow_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, yellow_start2, yellow_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, yellow_depth2, yellow_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
yellow_from = (
(yellow_depth1[0] + yellow_depth2[0]) / 2,
(yellow_depth1[1] + yellow_depth2[1]) / 2
) #calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(
yellow_start1[0] - yellow_end2[0],
yellow_start1[1] -
yellow_end2[1]) > math.hypot(
yellow_end1[0] - yellow_start2[0],
yellow_end1[1] - yellow_start2[1]):
yellow_to = (
(yellow_end1[0] + yellow_start2[0]) / 2,
(yellow_end1[1] + yellow_start2[1]) / 2)
else:
yellow_to = (
(yellow_start1[0] + yellow_end2[0]) / 2,
(yellow_start1[1] + yellow_end2[1]) / 2)
# print cv.ContourArea(seq)
cv.Line(dst, yellow_from, yellow_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst, yellow_from, 1,
cv.CV_RGB(255, 0, 0), 2, 8, 0)
cv.Circle(dst, yellow_to, 1, cv.CV_RGB(0, 0, 0), 2,
8, 0)
cv.Line(dst2, yellow_from, yellow_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst2, yellow_from, 1,
cv.CV_RGB(255, 0, 0), 2, 8, 0)
cv.Circle(dst2, yellow_to, 1,
cv.CV_RGB(255, 255, 255), 2, 8, 0)
break
else:
seq = seq.h_next()
continue
if (count > yellowcount or seq == None):
yellow_from = (0, 0)
yellow_to = (0, 0)
print "# error: yellow not found"
ballpos = (ballx, bally)
ShowImage("camera", dst)
if (found_goals == False):
if (us == "yellow"):
goals = find_goals(size, yellow_from)
stewies_goal = goals[0]
loiss_goal = goals[1]
found_goals = True
elif (us == "blue"):
goals = find_goals(size, blue_from)
stewies_goal = goals[0]
loiss_goal = goals[1]
found_goals = True
#if (ballx >= 0):
output(ballpos, blue_from, blue_to, yellow_from, yellow_to,
stewies_goal, loiss_goal)
time_passed = time.time() - aa
countf += 1
if (time_passed >= 1):
print "frame per second: " + str(countf),
countf = 0
aa = time.time()
cv.WaitKey(2)
random.randrange(0, 100) * 0.05 + 0.01,
random.randrange(0, 5) * 0.1,
random.randrange(0, 10),
line_type)
cv.PutText(image, "Testing text rendering!",
pt1, font,
random_color(random))
cv.ShowImage(window_name, image)
cv.WaitKey(delay)
# prepare a text, and get it's properties
font = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX,
3, 3, 0.0, 5, line_type)
text_size, ymin = cv.GetTextSize("OpenCV forever!", font)
pt1 = ((width - text_size[0]) / 2, (height + text_size[1]) / 2)
image2 = cv.CloneImage(image)
# now, draw some OpenCV pub ;-)
for i in range(0, 512, 2):
cv.SubS(image2, cv.ScalarAll(i), image)
(r, g, b) = colorsys.hsv_to_rgb((i % 100) / 100., 1, 1)
cv.PutText(image, "OpenCV forever!",
pt1, font, cv.RGB(255 * r, 255 * g, 255 * b))
cv.ShowImage(window_name, image)
cv.WaitKey(delay)
# wait some key to end
cv.WaitKey(0)
def process_frame(self, frame):
self.output.found = False
cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)
# Use RGB color finder
binary = libvision.cmodules.target_color_rgb.find_target_color_rgb(
frame, 250, 125, 0, 1500, 500, .3)
color_filtered = cv.CloneImage(binary)
blob_map = cv.CloneImage(binary)
blobs = libvision.blob.find_blobs(binary,
blob_map,
min_blob_size=50,
max_blobs=10)
if not blobs:
return
binary = cv.CloneImage(blob_map)
mapping = [0] * 256
for blob in blobs:
mapping[blob.id] = 255
libvision.greymap.greymap(blob_map, binary, mapping)
# Get Edges
cv.Canny(binary, binary, 30, 40)
# Hough Transform
line_storage = cv.CreateMemStorage()
lines = cv.HoughLines2(binary,
line_storage,
cv.CV_HOUGH_STANDARD,
rho=1,
theta=math.pi / 180,
threshold=self.hough_threshold,
param1=0,
param2=0)
print "hough transform found", len(lines), " lines"
lines = lines[:self.lines_to_consider] # Limit number of lines
# if not lines:
# return
paths = self.path_manager.process(lines, blobs)
if paths and not self.path:
# If path[1] is clockwise of paths[0]
distance = circular_distance(paths[0].angle, paths[1].angle)
if distance > 0:
self.path = paths[self.which_path]
else:
self.path = paths[1 - self.which_path]
if paths and self.path in paths and self.path.blobs:
temp_map = cv.CloneImage(blob_map)
mapping = [0] * 256
for blob in self.path.blobs:
mapping[blob.id] = 255
libvision.greymap.greymap(blob_map, temp_map, mapping)
center = self.find_centroid(temp_map)
svr.debug("map", temp_map)
self.path.center = (center[0] - (frame.width / 2),
-center[1] + (frame.height / 2))
random = 0
if random == 0:
# Show color filtered
color_filtered_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(color_filtered, color_filtered_rgb, cv.CV_GRAY2RGB)
cv.SubS(color_filtered_rgb, (255, 0, 0), color_filtered_rgb)
cv.Sub(frame, color_filtered_rgb, frame)
# Show edges
binary_rgb = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(binary, binary_rgb, cv.CV_GRAY2RGB)
cv.Add(frame, binary_rgb, frame) # Add white to edge pixels
cv.SubS(binary_rgb, (0, 0, 255), binary_rgb)
cv.Sub(frame, binary_rgb, frame) # Remove all but Red
test_lines = []
new_path = None
for line in lines[:]:
if self.candidates == []:
new_path = Path(line[0], line[1])
new_path.id = self.path_id
self.path_id += 1
new_path.last_seen += 1
self.candidates.append(new_path)
print "got a candidate"
for candidate in self.candidates:
if len(self.confirmed) == 0:
self.confirmed.append(candidate)
for line in lines[:]:
for candidate in self.candidates:
if math.fabs(line[0] - candidate.loc) < self.distance_threshold and \
math.fabs(line[1] - candidate.angle) < self.angle_threshold:
candidate.loc = (candidate.loc + line[0]) / 2
candidate.angle = (candidate.angle + line[1]) / 2
if candidate.last_seen < self.max_lastseen:
candidate.last_seen += 1
# print line1
if line in lines:
lines.remove(line)
else:
new_path = Path(line[0], line[1])
new_path.id = self.path_id
self.path_id += 1
new_path.last_seen += 1
new_path.seencount += 5
self.candidates.append(new_path)
for candidate in self.candidates[:]:
candidate.last_seen -= 1
if candidate.seencount > self.min_seencount:
self.confirmed.append(candidate)
self.candidates.remove(candidate)
if candidate.last_seen == -1:
self.candidates.remove(candidate)
for confirmed in self.confirmed:
for line in lines[:]:
if math.fabs(line[0] - confirmed.loc) < self.distance_trans and \
math.fabs(line[1] - confirmed.angle) < self.angle_trans:
confirmed.loc = line[0]
confirmed.angle = line[1]
if confirmed.last_seen < self.max_lastseen:
confirmed.last_seen += 2
if line in lines:
self.lines.remove(line)
print "line removed"
for confirmed in self.confirmed:
for candidate in self.candidates[:]:
if math.fabs(candidate.loc - confirmed.loc) < self.distance_trans and \
math.fabs(candidate.angle - confirmed.angle) < self.angle_trans:
confirmed.loc = candidate.loc
confirmed.angle = candidate.angle
if confirmed.last_seen < self.max_lastseen:
confirmed.last_seen += 2
print "lines"
if candidate in self.candidates:
self.candidates.remove(candidate)
print "line removed"
for confirmed1 in self.confirmed[:]:
for confirmed2 in self.confirmed[:]:
if math.fabs(confirmed1.loc - confirmed2.loc) < self.distance_threshold and \
math.fabs(confirmed1.angle - confirmed2.angle) < self.angle_threshold:
if confirmed1.id > confirmed2.id and confirmed1 in self.confirmed:
confirmed2.loc == (confirmed2.loc +
confirmed1.loc) / 2
confirmed2.angle == (confirmed2.angle +
confirmed1.angle) / 2
self.confirmed.remove(confirmed1)
if confirmed2.last_seen < self.max_lastseen:
confirmed2.last_seen += 2
if confirmed2.id > confirmed1.id and confirmed2 in self.confirmed:
confirmed2.loc == (confirmed2.loc +
confirmed1.loc) / 2
confirmed2.angle == (confirmed2.angle +
confirmed1.angle) / 2
self.confirmed.remove(confirmed2)
if confirmed1.last_seen < self.max_lastseen:
confirmed1.last_seen += 2
for confirmed in self.confirmed[:]:
confirmed.last_seen -= 1
if confirmed.last_seen < -10:
self.confirmed.remove(confirmed)
final_lines = []
for confirmed in self.confirmed:
final_line = [confirmed.loc, confirmed.angle]
final_lines.append(final_line)
print confirmed.id
candidate_ids = []
for candidate in self.candidates:
new_id = candidate.id
candidate_ids.append(new_id)
print candidate_ids
print len(self.candidates)
libvision.misc.draw_lines(frame, final_lines)
#libvision.misc.draw_lines2(frame, lines)
print "Number of Paths:", len(self.confirmed)
print "Number of Candidates:", len(self.candidates)
# type -s after the command to run vision for this to work and not produce errors.
# if len(self.confirmed)>1:
# raw_input()
self.output.paths = []
center_x = 0
center_y = 0
self.output.paths = self.confirmed
for path in self.output.paths:
path.theta = path.angle
center_x = frame.width / 2
path.x = center_x
center_y = (-math.cos(path.angle) /
(math.sin(path.angle) + .001)) * center_x + (
path.loc / ((math.sin(path.angle) + .001)))
path.y = center_y
if center_y > frame.height or center_y < 0 or \
center_y < self.min_center_distance or \
frame.height - center_y < self.min_center_distance:
center_y2 = frame.height / 2
center_x2 = (center_y2 -
(path.loc /
(math.sin(path.angle) + .0001))) / (
-math.cos(path.angle) /
(math.sin(path.angle) + .0001))
if center_x2 > frame.width or center_x2 < 0:
path.center = [center_x, center_y]
else:
path.center = [center_x2, center_y2]
else:
path.center = [center_x, center_y]
cv.Circle(frame, (int(path.center[0]), int(path.center[1])),
15, (255, 255, 255), 2, 8, 0)
self.return_output()
svr.debug("Path", frame)