def pipeline(self):
presentation = []
self.orig = self.source.grab_frame()
cv.Resize(self.orig, self.small)
size = self.smallsize
# store the raw image
presentation.append((self.small, 'raw'))
self.motion = cv.CloneImage(self.small)
# location of the thingy moving on the screen is defined by center
center = self.process_motion(self.motion)
# if an object is found draw the circle on the same location.
if center != (-1,-1):
cv.Circle(self.motion, center, cv.Round(30), cv.CV_RGB(0,225,0), 3, cv.CV_AA, 0)
# store the picture
presentation.append((self.motion, 'motion'))
####
# processing of final image
self.eye = cv.CloneImage(self.small) # get a copy
(r, color) = self.dejitter(center)
cv.Circle(self.eye, self.old_center, cv.Round(r),
color,
3, cv.CV_AA, 0)
presentation.append((self.eye, 'final'))
# combine and show the results
combined = self.combine_images(presentation)
cv.ShowImage('Motion detection', combined )
if self.store:
cv.WriteFrame(self.writer, self.combined)
def detect_faces(self):
self.frame()
# allocate temporary images
gray = cv.CreateImage((self.frame_copy.width, self.frame_copy.height),
8, 1)
small_img = cv.CreateImage(
(cv.Round(self.frame_copy.width / image_scale),
cv.Round(self.frame_copy.height / image_scale)), 8, 1)
# convert color input image to grayscale
cv.CvtColor(self.frame_copy, gray, cv.CV_BGR2GRAY)
# scale input image for faster processing
cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
cv.EqualizeHist(small_img, small_img)
faces = cv.HaarDetectObjects(small_img, self.cascade,
cv.CreateMemStorage(0), haar_scale,
min_neighbors, haar_flags, min_size)
return faces
def draw_linesC(src, lines, color, limit=None):
'''Draws lines on the image in given color, because I'm sick of red.
Arguments:
src - Source image which to draw lines on.
lines - A list of lines. This should be in the format of rho, theta
pairs: [(rho, theta), ...]. This is the same format which
cv.HoughLines2() returns.
limit - If given, it only that many number of lines will be drawn.
'''
if not limit:
limit = len(lines)
for rho, theta in lines[:limit]:
a = math.cos(theta)
b = math.sin(theta)
x0 = a * rho
y0 = b * rho
pt1 = (cv.Round(x0 + 1000 * (-b)), cv.Round(y0 + 1000 * (a)))
pt2 = (cv.Round(x0 - 1000 * (-b)), cv.Round(y0 - 1000 * (a)))
cv.Line(src, pt1, pt2, cv.RGB(color[0], color[1], color[2]), 3,
cv.CV_AA, 0)
def show_faces():
image = video_to_bgr(freenect.sync_get_video()[0])
min_size = (20, 20)
image_scale = 2
haar_scale = 1.2
min_neighbors = 2
haar_flags = 0
gray = cv.CreateImage((image.width, image.height), 8, 1)
small_image = cv.CreateImage((cv.Round(
image.width / image_scale), cv.Round(image.height / image_scale)), 8,
1)
cv.CvtColor(image, gray, cv.CV_BGR2GRAY)
cv.Resize(gray, small_image, cv.CV_INTER_LINEAR)
cv.EqualizeHist(small_image, small_image)
faces = cv.HaarDetectObjects(small_image, face_cascade,
cv.CreateMemStorage(0), haar_scale,
min_neighbors, haar_flags, min_size)
if faces:
for face in faces:
[X, y] = read_images('faces/',
(small_image.width, small_image.height))
y = np.asarray(y, dtype=np.int32)
model = cv2.createEigenFaceRecognizer()
model.train(np.asarray(X), np.asarray(y))
[p_label, p_confidence] = model.predict(
np.fromstring(small_image.tostring(), dtype=np.uint8))
# Print it:
print "Predicted label = %d (confidence=%.2f)" % (p_label,
p_confidence)
cv.ResetImageROI(image)
return image
def detect_and_draw(img, cascade):
# allocate temporary images
global var2
gray = cv.CreateImage((img.width, img.height), 8, 1)
small_img = cv.CreateImage((cv.Round(
img.width / image_scale), cv.Round(img.height / image_scale)), 8, 1)
# convert color input image to grayscale
cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
# scale input image for faster processing
cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
cv.EqualizeHist(small_img, small_img)
if (cascade):
t = cv.GetTickCount()
faces = cv.HaarDetectObjects(small_img, cascade,
cv.CreateMemStorage(0), haar_scale,
min_neighbors, haar_flags, min_size)
t = cv.GetTickCount() - t
# print "detection time = %gms" % (t/(cv.GetTickFrequency()*1000.))
if faces:
for ((x, y, w, h), n) in faces:
# the input to cv.HaarDetectObjects was resized, so scale the
# bounding box of each face and convert it to two CvPoints
pt1 = (int(x * image_scale), int(y * image_scale))
pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
cv.Rectangle(img, pt1, pt2, cv.RGB(255, 0, 0), 3, 8, 0)
print pt1
var2 = var2 + 1
if (var2 == 10):
led.face()
var2 = 0
emotion.happy()
cv.ShowImage("result", img)
def find_face(self, im):
im = im.convert("RGB")
cvim = cv.CreateImage(im.size, cv.IPL_DEPTH_8U, 3)
cv.SetData(cvim, im.tostring())
# allocate temporary images
gray = cv.CreateImage((cvim.width, cvim.height), 8, 1)
small_w = cv.Round(cvim.width / self.image_scale)
small_h = cv.Round(cvim.height / self.image_scale)
small_img = cv.CreateImage((small_w, small_h), 8, 1)
cv.CvtColor(cvim, gray, cv.CV_BGR2GRAY)
cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
cv.EqualizeHist(small_img, small_img)
#t = cv.GetTickCount()
faces = cv.HaarDetectObjects(small_img, self.cascade,
cv.CreateMemStorage(0), self.haar_scale,
self.min_neighbors, self.haar_flags,
self.min_size)
#t = cv.GetTickCount() - t
adjust = lambda v: v * self.image_scale
if faces:
for ((x, y, w, h), n) in faces:
yield adjust(x), adjust(y), adjust(w), adjust(h)
def detectFace(img, cascade):
gray = cv.CreateImage((img.width, img.height), 8, 1)
small_img = cv.CreateImage(
(cv.Round(img.width / imageScale), cv.Round(img.height / imageScale)),
8, 1)
cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
cv.EqualizeHist(small_img, small_img)
#faceCascade = cv2.CascadeClassifier(cascade1)
faces = cv.HaarDetectObjects(small_img, cascade, cv.CreateMemStorage(0),
haarScale, minNeighbors, haarFlags, minSize)
#faces =faceCascade.detectMultiScale(i,1,5,(30,30),cv.CV_HAAR_SCALE_IMAGE)
print "now it would check if face detected or not"
if faces:
print "detected", len(faces)
for ((x, y, w, h), n) in faces:
pt1 = (int(x * imageScale), int(y * imageScale))
pt2 = (int((x + w) * imageScale), int((y + h) * imageScale))
cv.Rectangle(img, pt1, pt2, cv.RGB(255, 0, 0), 3, 8, 0)
return img
else:
print "no face"
return False
def detectCircles(self, rect):
"""Detect circles in the picture
the Hough circle transform parameters min_radius and max_radius
are adjusted as follows (for the 768x576 image):
* The black direction marker is around 8 to 11 pixels wide.
* The ball is around 12 and 16 pixels wide.
The above estimates are taken with GIMP. The lower bounds are
obtained using the darker, inner pixels and the upper bounds using
the lighter edge pixels. Since the Hough circle transform is
resistant against damage (it could work with only half of the
circle present), we stick with radii obtained from these measures
and not try to "account for" the cases where the circles are
somehow obscured and look smaller to the eye.
"""
out = cv.CloneImage(rect)
cv.Smooth(rect, rect, cv.CV_GAUSSIAN, 9, 9)
size = cv.GetSize(rect)
cv.CvtColor(rect, self.gray8, cv.CV_BGR2GRAY)
#storage = cv.CreateMemStorage(0)
storage = cv.CreateMat(7000, 1, cv.CV_32FC3)
print "PARAMS:", self.hough_params
#Note: circles.total denotes the number of circles
circles = cv.HoughCircles(
self.gray8,
storage,
cv.CV_HOUGH_GRADIENT,
3, #dp / resolution
2, #circle dist threshold
max(1, self.hough_params[0]), #param1
max(1, self.hough_params[1]), #param2
2, #min_radius
14, #max_radius
)
for x, y, radius in [storage[i, 0] for i in range(storage.rows)]:
cv.Circle(rect, (x, y), cv.Round(radius), cv.CV_RGB(300, 1, 1))
cv.ShowImage("BASD", rect)
return rect
def update(self, im=None):
if im is not None:
self.im = im
cv.CalcArrHist([self.im], self.hist)
(min_value, max_value, _, _) = cv.GetMinMaxHistValue(self.hist)
cv.Scale(self.hist.bins, self.hist.bins,
float(self.hist_image.height) / max_value, 0)
cv.Set(self.hist_image, cv.ScalarAll(255))
bin_w = round(float(self.hist_image.width) / self.hist_size)
for i in range(self.hist_size):
cv.Rectangle(self.hist_image,
(int(i * bin_w), self.hist_image.height),
(int((i + 1) * bin_w), self.hist_image.height -
cv.Round(self.hist.bins[i])), self.color, -1, 8, 0)
cv.ShowImage(self.title, self.hist_image)
def get_hist_image(hist, bins, width=500):
height = 255
white = cv.RGB(255, 255, 255)
black = cv.RGB(0, 0, 0)
img_size = (width, height)
hist_img = cv.CreateImage(img_size, 8, 1)
cv.Rectangle(hist_img, (0, 0), img_size, white, cv.CV_FILLED)
(_, max_value, _, _) = cv.GetMinMaxHistValue(hist)
scale = width / float(bins)
x = 0
for s in range(bins):
bin_val = cv.QueryHistValue_1D(hist, s)
y = cv.Round(bin_val * height / max_value)
cv.Rectangle(hist_img, (x, height - y), (x + scale, height), black,
cv.CV_FILLED)
x += scale
return hist_img
def get_2d_hist_img(x_bins=30, y_bins=32, scale=10, hist=None, img=None):
if not hist:
if img:
hist = get_hs_2d_hist(img, x_bins, y_bins)
else:
raise Exception("Histogram or image should be given")
(_, max_value, _, _) = cv.GetMinMaxHistValue(hist)
hist_img = cv.CreateImage((x_bins * scale, y_bins * scale), 8, 3)
for h in range(x_bins):
for s in range(y_bins):
bin_val = cv.QueryHistValue_2D(hist, h, s)
intensity = cv.Round(bin_val * 255 / max_value)
cv.Rectangle(
hist_img, (h * scale, s * scale),
((h + 1) * scale - 1, (s + 1) * scale - 1),
cv.RGB(255 - intensity, 255 - intensity,
255 - intensity), cv.CV_FILLED)
return hist_img
def OCVHistogram(frame, ranges=[[0, 256]], hist_size=64):
"""Create a histogram of given frame"""
if frame.nChannels != 1:
dest = OCVCopyGrayscale(frame)
else:
dest = frame
hist_image = cv.CreateImage((dest.width, dest.height), 8, 1)
hist = cv.CreateHist([hist_size], cv.CV_HIST_ARRAY, ranges, 1)
cv.CalcArrHist([dest], hist)
(min_value, max_value, _, _) = cv.GetMinMaxHistValue(hist)
cv.Scale(hist.bins, hist.bins, float(hist_image.height) / max_value, 0)
cv.Set(hist_image, cv.ScalarAll(255))
bin_w = round(float(hist_image.width) / hist_size)
for i in range(hist_size):
cv.Rectangle(hist_image, (int(i * bin_w), hist_image.height), (int(
(i + 1) * bin_w), hist_image.height - cv.Round(hist.bins[i])),
cv.ScalarAll(0), -1, 8, 0)
return hist_image
def hs_histogram(src):
# Convert to HSV
hsv = cv.CreateImage(cv.GetSize(src), 8, 3)
cv.CvtColor(src, hsv, cv.CV_BGR2HSV)
# Extract the H and S planes
h_plane = cv.CreateMat(src.rows, src.cols, cv.CV_8UC1)
s_plane = cv.CreateMat(src.rows, src.cols, cv.CV_8UC1)
cv.Split(hsv, h_plane, s_plane, None, None)
planes = [h_plane, s_plane]
h_bins = 30
s_bins = 32
hist_size = [h_bins, s_bins]
# hue varies from 0 (~0 deg red) to 180 (~360 deg red again */
h_ranges = [0, 180]
# saturation varies from 0 (black-gray-white) to
# 255 (pure spectrum color)
s_ranges = [0, 255]
ranges = [h_ranges, s_ranges]
scale = 10
hist = cv.CreateHist([h_bins, s_bins], cv.CV_HIST_ARRAY, ranges, 1)
cv.CalcHist([cv.GetImage(i) for i in planes], hist)
(_, max_value, _, _) = cv.GetMinMaxHistValue(hist)
hist_img = cv.CreateImage((h_bins*scale, s_bins*scale), 8, 3)
for h in range(h_bins):
for s in range(s_bins):
bin_val = cv.QueryHistValue_2D(hist, h, s)
intensity = cv.Round(bin_val * 255 / max_value)
cv.Rectangle(hist_img,
(h*scale, s*scale),
((h+1)*scale - 1, (s+1)*scale - 1),
cv.RGB(intensity, intensity, intensity),
cv.CV_FILLED)
return hist_img
# This is the new part here. ie Use of cv.BoundingRect()
while contour:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(contour))
contour = contour.h_next()
print contour
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(color_image, pt1, pt2, cv.CV_RGB(255, 0, 0), 1)
# Calculating centroids
centroidx = cv.Round((pt1[0] + pt2[0]) / 2)
centroidy = cv.Round((pt1[1] + pt2[1]) / 2)
# Identifying if blue or yellow blobs and adding centroids to corresponding lists
if (20 < cv.Get2D(imghsv, centroidy, centroidx)[0] < 30):
yellow.append((centroidx, centroidy))
elif (100 < cv.Get2D(imghsv, centroidy, centroidx)[0] < 120):
blue.append((centroidx, centroidy))
# Now drawing part. Exceptional handling is used to avoid IndexError. After drawing is over, centroid from previous part is # removed from list by pop. So in next frame,centroids in this frame become initial points of line to draw.
try:
cv.Circle(imdraw, yellow[1], 5, (0, 255, 255))
cv.Line(imdraw, yellow[0], yellow[1], (0, 255, 255), 3, 8, 0)
yellow.pop(0)
except IndexError:
print "Just wait for yellow"
def eyefinder(shm_tar, mutex_tar,IP,PORT,fighter):
global motionProxy
global post
global sonarProxy
global memoryProxy
global cameraProxy
global videoClient
# work ! set current to servos
stiffnesses = 1.0
time.sleep(0.5)
# TEMP
pNames = "Body"
pStiffnessLists = 0.0
pTimeLists = 1.0
motionProxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = cameraProxy.getImageRemote(videoClient)
imageWidth = naoImage[0]
imageHeight = naoImage[1]
found = True
posx=0
posy=0
mem = cv.CreateMemStorage(0)
i=0
cv.NamedWindow("Real")
cv.MoveWindow("Real",0,0)
cv.NamedWindow("Threshold")
cv.MoveWindow("Real",imageWidth+100,0)
error=0.0
nframe=0.0
closing = 3
tstp,tu=0,0
K=2
try:
while found:
#Synchro
mutex_tar.acquire()
n = shm_tar.value[0]
mutex_tar.release()
if n==-1:
print "Fail in target mem zone tar. Exit" , n
elif n==-2:
print "Terminated by parent"
nframe=nframe+1
# Get current image (top cam)
naoImage = cameraProxy.getImageRemote(videoClient)
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
pilImg = Image.fromstring("RGB", (imageWidth, imageHeight), array)
# Convert Image to OpenCV
cvImg = cv.CreateImageHeader((imageWidth, imageHeight),cv.IPL_DEPTH_8U, 3)
cv.SetData(cvImg, pilImg.tostring())
cv.CvtColor(cvImg, cvImg, cv.CV_RGB2BGR)
hsv_img = cv.CreateImage(cv.GetSize(cvImg), 8, 3)
cv.CvtColor(cvImg, hsv_img, cv.CV_BGR2HSV)
thresholded_img = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
thresholded_img2 = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
thresholded1 = cv.CreateImage(cv.GetSize(cvImg), 8, 1)
cv.InRangeS(hsv_img, (91, 130, 130), (130, 255, 255), thresholded1)
storage2 = cv.CreateMemStorage(0)
contour2 = cv.FindContours(thresholded1, storage2, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
points = []
d=[]
data2=[]
while contour2:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(contour2))
contour2 = contour2.h_next()
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(cvImg, pt1, pt2, cv.CV_RGB(255,0,0), 1)
lastx=posx
lasty=posy
posx=cv.Round((pt1[0]+pt2[0])/2)
posy=cv.Round((pt1[1]+pt2[1])/2)
data2.append([posx,posy])
d.append(math.sqrt(pt1[0]**2+pt2[0]**2))
d.append(math.sqrt(pt1[1]**2+pt2[1]**2))
cvImg,error,centroid1,labels = clustering(data2,cvImg,nframe,error,K=1)
thresholded2 = cv.CreateImage(cv.GetSize(cvImg), 8, 1)
cv.InRangeS(hsv_img, (70, 150, 150), (89, 255, 255), thresholded2)
storage2 = cv.CreateMemStorage(0)
contour2 = cv.FindContours(thresholded2, storage2, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
points = []
d=[]
data2=[]
while contour2:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(contour2))
contour2 = contour2.h_next()
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(cvImg2, pt1, pt2, cv.CV_RGB(255,0,0), 1)
lastx=posx
lasty=posy
posx=cv.Round((pt1[0]+pt2[0])/2)
posy=cv.Round((pt1[1]+pt2[1])/2)
data2.append([posx,posy])
d.append(math.sqrt(pt1[0]**2+pt2[0]**2))
d.append(math.sqrt(pt1[1]**2+pt2[1]**2))
cvImg2,error,centroid2,labels = clustering(data2,cvImg2,nframe,error,K=1)
if (len(centroid2)!=0 and len(centroid1)!=0):
c1 = centroid1.tolist()[0]
c2 = centroid2.tolist()[0]
if c1[0]>c2[0]:
C = [c1,c2]
else:
C = [c2,c1]
#print C
mutex_tar.acquire()
shm_tar.value = [n,C[0],C[1]]
mutex_tar.release()
cv.WaitKey(1)
except KeyboardInterrupt:
print
print "Interrupted by user, shutting down"
end(IP,PORT)
frame = cv.QueryFrame(capture)
if not frame:
cv.WaitKey(0)
break
if not inputFrame:
inputFrame = cv.CreateImage((frame.width, frame.height),
cv.IPL_DEPTH_8U, frame.nChannels)
if frame.origin == cv.IPL_ORIGIN_TL:
cv.Copy(frame, inputFrame)
else:
cv.Flip(frame, inputFrame, 0)
#### resize and convert images ####
# resized color input image
frameSmall = cv.CreateImage(
(cv.Round(inputFrame.width * imageResize),
cv.Round(inputFrame.height * imageResize)), 8,
inputFrame.nChannels)
cv.Resize(inputFrame, frameSmall, cv.CV_INTER_LINEAR)
# convert resized color input image to grayscale
frameSmallGray = cv.CreateImage(
(frameSmall.width, frameSmall.height), 8, 1)
cv.CvtColor(frameSmall, frameSmallGray, cv.CV_BGR2GRAY)
cv.EqualizeHist(frameSmallGray, frameSmallGray)
# convert resized color input image to grayscale
frameSmallHSV = cv.CreateImage(
(frameSmall.width, frameSmall.height), 8, 3)
cv.CvtColor(frameSmall, frameSmallHSV, cv.CV_BGR2HSV)
###################################
hBins = 30
def swordcenterdetection(enemy):
global motionProxy
global post
global sonarProxy
global memoryProxy
# work ! set current to servos
stiffnesses = 1.0
time.sleep(0.5)
# init video
cameraProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = 1 # 0 : QQVGA, 1 : QVGA, 2 : VGA
colorSpace = 11 # RGB
camNum = 0 # 0:top cam, 1: bottom cam
fps = 1; # frame Per Second
cameraProxy.setParam(18, camNum)
try:
videoClient = cameraProxy.subscribe("python_client",
resolution, colorSpace, fps)
except:
cameraProxy.unsubscribe("python_client")
videoClient = cameraProxy.subscribe("python_client",
resolution, colorSpace, fps)
print "Start videoClient: ",videoClient
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = cameraProxy.getImageRemote(videoClient)
imageWidth = naoImage[0]
imageHeight = naoImage[1]
found = True
posx=0
posy=0
mem = cv.CreateMemStorage(0)
i=0
cv.NamedWindow("Real")
cv.MoveWindow("Real",0,0)
cv.NamedWindow("Threshold")
cv.MoveWindow("Real",imageWidth+100,0)
error=0.0
nframe=0.0
closing = 1
tstp,tu=0,0
K=2
pb = 0.5 # low pass filter value
try:
while found:
nframe=nframe+1
# Get current image (top cam)
naoImage = cameraProxy.getImageRemote(videoClient)
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
pilImg = Image.fromstring("RGB", (imageWidth, imageHeight), array)
# Convert Image to OpenCV
cvImg = cv.CreateImageHeader((imageWidth, imageHeight),cv.IPL_DEPTH_8U, 3)
cv.SetData(cvImg, pilImg.tostring())
cv.CvtColor(cvImg, cvImg, cv.CV_RGB2BGR)
hsv_img = cv.CreateImage(cv.GetSize(cvImg), 8, 3)
cv.CvtColor(cvImg, hsv_img, cv.CV_BGR2HSV)
thresholded_img = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
thresholded_img2 = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
temp = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
eroded = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
skel = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
img = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
edges = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
# Get the orange on the image
cv.InRangeS(hsv_img, (110, 80, 80), (150, 200, 200), thresholded_img)
storage = cv.CreateMemStorage(0)
lines = cv.HoughLines2(thresholded_img, storage, cv.CV_HOUGH_STANDARD, 1, cv.CV_PI/180, 100, param1=0, param2=0)
first = 1
sl=0
Mx=[]
My=[]
for l in lines:
sl=sl+1
for i in range((sl-1)):
l=lines[i]
rho = l[0]
theta = l[1]
a = np.cos(theta)
b = np.sin(theta)
x0 = a*rho
y0 = b*rho
cf1,cf2 = 300,300
xpt11 = int(cv.Round(x0 + cf1*(-b)))
ypt11 = int(cv.Round(y0 + cf1*(a)))
xpt12 = int(cv.Round(x0 - cf2*(-b)))
ypt12 = int(cv.Round(y0 - cf2*(a)))
pt11 = (xpt11,ypt11)
pt12 = (xpt12,ypt12)
cv.Line(cvImg, pt11, pt12, cv.CV_RGB(255,255,255), thickness=1, lineType=8, shift=0)
l=lines[(i+1)]
rho = l[0]
theta = l[1]
a = np.cos(theta)
b = np.sin(theta)
x0 = a*rho
y0 = b*rho
cf1,cf2 = 300,300
xpt1 = int(cv.Round(x0 + cf1*(-b)))
ypt1 = int(cv.Round(y0 + cf1*(a)))
xpt2 = int(cv.Round(x0 - cf2*(-b)))
ypt2 = int(cv.Round(y0 - cf2*(a)))
A = np.array(((xpt1,ypt1),(xpt2,ypt2)))
B = np.array(((xpt11,ypt11),(xpt12,ypt12)))
try:
m = line_intersection(A, B)
mx = m[0]
my = m[1]
Mx.append(mx)
My.append(my)
except:
error=1 #intersection return False we don't add the point
pt1 = (xpt1,ypt1)
pt2 = (xpt2,ypt2)
cv.Line(cvImg, pt1, pt2, cv.CV_RGB(255,255,255), thickness=1, lineType=8, shift=0)
cMx,cMy=[],[]
for x in Mx:
cMx.append((1-pb)*x)
for y in My:
cMy.append((1-pb)*y)
try:
for i in range(len(cMx)):
Mx[i] = cMx[i]+cMtx[i]
My[i] = cMy[i]+cMty[i]
Mm = (int(np.mean(Mx)),int(np.mean(My)))
print "M",Mm
cv.Circle(cvImg,Mm,5,(254,0,254),-1)
except:
error=1 # we are at first iteration
cMtx,cMty=[],[]
Mtx = Mx
Mty = My
for x in Mtx:
cMtx.append(pb*x)
for y in Mty:
cMty.append(pb*y)
cv.ShowImage("Real",cvImg)
cv.ShowImage("Threshold",thresholded_img)
cv.WaitKey(1)
except KeyboardInterrupt:
print
print "Interrupted by user, shutting down"
end()
def meet(IP, PORT, fighter):
global motionProxy
global post
global sonarProxy
global memoryProxy
global cameraProxy
global videoClient
# work ! set current to servos
stiffnesses = 1.0
time.sleep(0.5)
# TEMP
pNames = "Body"
pStiffnessLists = 0.0
pTimeLists = 1.0
motionProxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists)
# Get a camera image.
# image[6] contains the image data passed as an array of ASCII chars.
naoImage = cameraProxy.getImageRemote(videoClient)
imageWidth = naoImage[0]
imageHeight = naoImage[1]
found = True
posx = 0
posy = 0
mem = cv.CreateMemStorage(0)
i = 0
cv.NamedWindow("Real")
cv.MoveWindow("Real", 0, 0)
cv.NamedWindow("Threshold")
cv.MoveWindow("Real", imageWidth + 100, 0)
error = 0.0
nframe = 0.0
closing = 3
tstp, tu = 0, 0
K = 2
try:
while found:
nframe = nframe + 1
# Get current image (top cam)
naoImage = cameraProxy.getImageRemote(videoClient)
# Get the image size and pixel array.
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
# Create a PIL Image from our pixel array.
pilImg = Image.fromstring("RGB", (imageWidth, imageHeight), array)
# Convert Image to OpenCV
cvImg = cv.CreateImageHeader((imageWidth, imageHeight),
cv.IPL_DEPTH_8U, 3)
cv.SetData(cvImg, pilImg.tostring())
cvImg2 = cv.CreateImageHeader((imageWidth, imageHeight),
cv.IPL_DEPTH_8U, 3)
cv.SetData(cvImg2, pilImg.tostring())
cv.CvtColor(cvImg, cvImg, cv.CV_RGB2BGR)
hsv_img = cv.CreateImage(cv.GetSize(cvImg), 8, 3)
cv.CvtColor(cvImg, hsv_img, cv.CV_BGR2HSV)
thresholded_img = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
thresholded_img2 = cv.CreateImage(cv.GetSize(hsv_img), 8, 1)
imcv2 = cv2.cvtColor(np.asarray(pilImg), cv2.COLOR_RGB2BGR)
# Get the orange on the image
cv.InRangeS(hsv_img, (0, 150, 150), (40, 255, 255),
thresholded_img)
cv.InRangeS(hsv_img, (0, 150, 150), (40, 255, 255),
thresholded_img2)
storage = cv.CreateMemStorage(0)
contour = cv.FindContours(thresholded_img, storage,
cv.CV_RETR_CCOMP,
cv.CV_CHAIN_APPROX_SIMPLE)
cv.Smooth(thresholded_img, thresholded_img, cv.CV_GAUSSIAN, 5, 5)
cv.Erode(thresholded_img, thresholded_img, None, closing)
cv.Dilate(thresholded_img, thresholded_img, None, closing)
storage = cv.CreateMemStorage(0)
contour = cv.FindContours(thresholded_img, storage,
cv.CV_RETR_CCOMP,
cv.CV_CHAIN_APPROX_SIMPLE)
points = []
d = []
data = []
while contour:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(contour))
contour = contour.h_next()
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2],
bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(cvImg2, pt1, pt2, cv.CV_RGB(255, 0, 0), 1)
lastx = posx
lasty = posy
posx = cv.Round((pt1[0] + pt2[0]) / 2)
posy = cv.Round((pt1[1] + pt2[1]) / 2)
data.append([posx, posy])
d.append(math.sqrt(pt1[0]**2 + pt2[0]**2))
d.append(math.sqrt(pt1[1]**2 + pt2[1]**2))
cvImg2, error, centroid, labels = clustering(
data, cvImg2, nframe, error, K)
# Update the closing size towards the number of found labels
if labels.size < 2:
closing = 1
if labels.size < 6:
closing = 2
if labels.size > 10:
closing = 3
if closing < 1:
closing = 0
if centroid.size != 0:
uh = 0
try:
x = int(centroid[0][0])
y = int(centroid[0][1])
except:
print "NaN"
l = memoryProxy.getData(
"Device/SubDeviceList/US/Left/Sensor/Value")
r = memoryProxy.getData(
"Device/SubDeviceList/US/Right/Sensor/Value")
# Commande proportionnelles pour le cap et la distance
kh = 20.0 / (imageWidth / 2)
km = 0.25
uh = -kh * (x - imageWidth / 2)
um = km * (math.sqrt(l**2 + r**2) - 0.6)
if (uh > 4 or uh < -4):
motionProxy.moveTo(0.0, 0.0, uh * almath.TO_RAD)
tu = time.time()
if (um > 0.03 or um < -0.03) and (l > 0.6 and r > 0.6):
motionProxy.moveTo(um, 0.0, 0.0)
tu = time.time()
else:
# quand il est proche mettre K=1, on admet qu il n y a plus de parasites, et que les zones oranges sont assez
# grosse pour leur appliquer un coef ce fermeture eleve
# print "-------------------------"
# print "K = 1"
K = 1
closing = 5
tstp = time.time()
# print "l",l
# print "r",r
# print "um ",um
#Temps de repos du Nao
tact = tstp - tu
#S'il attend plus de 5sec, il admet qu'il est devant sa cible
if tact > 5:
found = False
if fighter == "dark":
#tts.say("I've been waiting for you, Obi-Wan. We meet again, at last. The circle is now complete. When I left you, I was but the learner; now I am the master.")
#time.sleep(1.0)
tts.say("I see you")
if fighter == "obi":
#time.sleep(15.0)
#tts.say("Only a master of evil, Darth.")
tts.say("I see you")
cv.ShowImage("Real", cvImg)
#cv.ShowImage("Threshold",thresholded_img2)
cv.ShowImage("Threshold", thresholded2)
cv.WaitKey(1)
except KeyboardInterrupt:
print
print "Interrupted by user, shutting down"
end(IP, PORT)
frame_no = 0
while (frame_no < __start__):
frame = cv.QueryFrame(capture)
frame_no += 1
while 1:
# capture the current frame
frame = cv.QueryFrame(capture)
if frame:
gray = cv.CreateImage((frame.width, frame.height), 8, 1)
cv.CvtColor(frame, gray, cv.CV_BGR2GRAY)
# scale input image for faster processing
small_img = cv.CreateImage((cv.Round(
frame.width / __scale__), cv.Round(frame.height / __scale__)),
8, 1)
cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
cv.EqualizeHist(small_img, small_img)
pi = Image.fromstring("L", cv.GetSize(small_img),
small_img.tostring())
s_res = sift(ravel(PIL2NumPy(pi)))
n_res = array(s_res)
for item in n_res:
xx = item[0] * __scale__
yy = item[1] * __scale__
pt = (int(xx), int(yy))
if is_point_in_region(pt):
cv.Circle(frame, pt, 8, cv.CV_RGB(100, 100, 255), 0,
cv.CV_AA, 0)
def getPx(img, x, y):
width, height = cv.GetSize(img)
x = min(max(0, x), width - 1)
y = min(max(0, y), height - 1)
srchPt = cv.Round(y), cv.Round(x)
return img[srchPt]
cv.NamedWindow("threshold")
cv.NamedWindow("output")
while (1):
frame = cv.QueryFrame(capture)
cv.Flip(
frame, frame, 1
) # Horizontal flipping for synchronization, comment it to see difference.
imdraw = cv.CreateImage(cv.GetSize(frame), 8,
3) # We make all drawings on imdraw.
thresh_img = getthresholdedimg(frame) # We get coordinates from thresh_img
cv.Erode(thresh_img, thresh_img, None, 1) # Eroding removes small noises
(leftmost, rightmost, topmost, bottommost) = getpositions(thresh_img)
if (leftmost - rightmost != 0) or (topmost - bottommost != 0):
lastx = posx
lasty = posy
posx = cv.Round((rightmost + leftmost) / 2)
posy = cv.Round((bottommost + topmost) / 2)
if lastx != 0 and lasty != 0:
cv.Line(imdraw, (posx, posy), (lastx, lasty), (b, g, r))
cv.Circle(imdraw, (posx, posy), 5, (b, g, r), -1)
cv.Add(
test, imdraw, test
) # Adding imdraw on test keeps all lines there on the test frame. If not, we don't get full drawing, instead we get only that fraction of line at the moment.
cv.ShowImage("threshold", thresh_img)
cv.ShowImage("output", test)
if cv.WaitKey(33) == 1048603: # Exit if Esc key is pressed
break
cv.DestroyWindow("output") # Releasing window
cv.DestroyWindow("threshold")
def DetectRedEyes(image, faceCascade, smileCascade):
min_size = (20, 20)
image_scale = 2
haar_scale = 1.2
min_neighbors = 2
haar_flags = 0
# Allocate the temporary images
gray = cv.CreateImage((image.width, image.height), 8, 1)
smallImage = cv.CreateImage((cv.Round(
image.width / image_scale), cv.Round(image.height / image_scale)), 8,
1)
# Convert color input image to grayscale
cv.CvtColor(image, gray, cv.CV_BGR2GRAY)
# Scale input image for faster processing
cv.Resize(gray, smallImage, cv.CV_INTER_LINEAR)
# Equalize the histogram
cv.EqualizeHist(smallImage, smallImage)
# Detect the faces
faces = cv.HaarDetectObjects(smallImage, faceCascade,
cv.CreateMemStorage(0), haar_scale,
min_neighbors, haar_flags, min_size)
# If faces are found
if faces:
print faces
for ((x, y, w, h), n) in faces:
# the input to cv.HaarDetectObjects was resized, so scale the
# bounding box of each face and convert it to two CvPoints
print "face"
pt1 = (int(x * image_scale), int(y * image_scale))
pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
# print pt1
# print pt2
cv.Rectangle(image, pt1, pt2, cv.RGB(255, 0, 0), 1, 8, 0)
cv.PutText(image, "face", pt1, font, cv.RGB(255, 0, 0))
face_region = cv.GetSubRect(image,
(x, int(y + (h / 4)), w, int(h / 2)))
#split face
cv.Rectangle(image,
(pt1[0], (pt1[1] + (abs(pt1[1] - pt2[1]) / 2))), pt2,
cv.RGB(0, 255, 0), 1, 8, 0)
cv.PutText(image, "lower",
(pt1[0], (pt1[1] + (abs(pt1[1] - pt2[1]) / 2))), font,
cv.RGB(0, 255, 0))
cv.SetImageROI(
image, (pt1[0],
(pt1[1] + (abs(pt1[1] - pt2[1]) / 2)), pt2[0] - pt1[0],
int((pt2[1] - (pt1[1] + (abs(pt1[1] - pt2[1]) / 2))))))
smiles = cv.HaarDetectObjects(image, smileCascade,
cv.CreateMemStorage(0), 1.1, 5, 0,
(15, 15))
if smiles:
#print smiles
for smile in smiles:
cv.Rectangle(
image, (smile[0][0], smile[0][1]),
(smile[0][0] + smile[0][2], smile[0][1] + smile[0][3]),
cv.RGB(0, 0, 255), 1, 8, 0)
cv.PutText(image, "smile", (smile[0][0], smile[0][1]),
font, cv.RGB(0, 0, 255))
cv.PutText(image, str(smile[1]),
(smile[0][0], smile[0][1] + smile[0][3]), font,
cv.RGB(0, 0, 255))
#print ((abs(smile[0][1] - smile[0][2]) / abs(pt1[0] - pt2[0])) * 100)
global smileness
smileness = smile[1]
cv.ResetImageROI(image)
#if smile[1] > 90:
# mqttc.publish("smiles", "got smile", 1)
# time.sleep(5)
#eyes = cv.HaarDetectObjects(image, eyeCascade,
#cv.CreateMemStorage(0),
#haar_scale, min_neighbors,
#haar_flags, (15,15))
#if eyes:
# For each eye found
#print eyes
#for eye in eyes:
# Draw a rectangle around the eye
# cv.Rectangle(image,
# (eye[0][0],
# eye[0][1]),
# (eye[0][0] + eye[0][2],
# eye[0][1] + eye[0][3]),
# cv.RGB(255, 0, 0), 1, 8, 0)
cv.ResetImageROI(image)
return image
def getthresholdedimgR4(imhsv):
# A little change here. Creates images for blue and yellow (or whatever color you like).
imgRed = cv.CreateImage(cv.GetSize(imhsv), 8, 1)
imgYellow = cv.CreateImage(cv.GetSize(imhsv), 8, 1)
imgGreen = cv.CreateImage(cv.GetSize(imhsv), 8, 1)
imgthreshold = cv.CreateImage(cv.GetSize(imhsv), 8, 1)
cv.InRangeS(imghsv, cv.Scalar(0, 190, 130), cv.Scalar(18, 255, 190),
imgRed) # Select a range of red color
cv.InRangeS(imghsv, cv.Scalar(100, 100, 100), cv.Scalar(120, 255, 255),
imgYellow) # Select a range of blue color
cv.InRangeS(imghsv, cv.Scalar(67, 103, 46), cv.Scalar(100, 209, 184),
imgGreen) # Select a range of green color
storage = cv.CreateMemStorage(0)
redContour = cv.FindContours(imgRed, storage, cv.CV_RETR_CCOMP,
cv.CV_CHAIN_APPROX_SIMPLE)
points = []
while redContour:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(redContour))
#bound_rect = cv.BoundingRect(contour)
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(imhsv, pt1, pt2, cv.CV_RGB(255, 0, 0), 1)
#Calculating centroids
centroidx = cv.Round((pt1[0] + pt2[0]) / 2)
centroidy = cv.Round((pt1[1] + pt2[1]) / 2)
area = cv.ContourArea(list(redContour))
redContour = redContour.h_next()
while yellowContour:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(redContour))
#bound_rect = cv.BoundingRect(contour)
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(imhsv, pt1, pt2, cv.CV_RGB(255, 0, 0), 1)
#Calculating centroids
centroidx = cv.Round((pt1[0] + pt2[0]) / 2)
centroidy = cv.Round((pt1[1] + pt2[1]) / 2)
area = cv.ContourArea(list(redContour))
redContour = redContour.h_next()
cv.ShowImage("Ceva", imhsv)
cv.Add(imgRed, imgBlue, imgthreshold)
cv.Add(imgGreen, imgGreen, imgthreshold)
return imgthreshold
cv.CV_CHAIN_APPROX_SIMPLE)
points = []
# This is the new part here. ie Use of cv.BoundingRect()
while contour:
# Draw bounding rectangles
bound_rect = cv.BoundingRect(list(contour))
contour = contour.h_next()
# for more details about cv.BoundingRect,see documentation
pt1 = (bound_rect[0], bound_rect[1])
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
points.append(pt1)
points.append(pt2)
cv.Rectangle(color_image, pt1, pt2, cv.CV_RGB(255, 0, 0), 1)
lastx = posx
lasty = posy
posx = cv.Round((pt1[0] + pt2[0]) / 2)
posy = cv.Round((pt1[1] + pt2[1]) / 2)
if lastx != 0 and lasty != 0:
cv.Line(imdraw, (posx, posy), (lastx, lasty), (0, 255, 255))
cv.Circle(imdraw, (posx, posy), 5, (0, 255, 255), -1)
cv.Add(test, imdraw, test)
cv.ShowImage("Real", color_image)
cv.ShowImage("Threshold", test)
if cv.WaitKey(33) == 1048603:
cv.DestroyWindow("Real")
cv.DestroyWindow("Threshold")
break
def label_sift_point(image, xx, yy, label):
color = cv.CV_RGB(255, 255, 255)
center = (int(xx), int(yy))
cv.Circle(image, center, cv.Round(4), color, 3, cv.CV_AA, 0)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 0.1, 0, 1, cv.CV_AA)
cv.PutText(image, label, center, font, cv.CV_RGB(255, 255, 0))
hist = cv.CreateHist([h_bins, s_bins], cv.CV_HIST_ARRAY, [(0, 180),
(0, 255)], 1)
cv.CalcHist(planes, hist, 0, None)
cv.NormalizeHist(hist, 1.0)
# Create an image to use to visualize our histogram.
scale = 10
hist_img = cv.CreateImage((h_bins * scale, s_bins * scale), 8, 3)
cv.Zero(hist_img)
max_value = 0
(minvalue, maxvalue, minidx, maxidx) = cv.GetMinMaxHistValue(hist)
for h in range(h_bins):
for s in range(s_bins):
bin_val = cv.QueryHistValue_2D(hist, h, s)
intensity = cv.Round(bin_val * 255 / maxvalue)
cv.Rectangle(hist_img, (h * scale, s * scale),
((h + 1) * scale - 1, (s + 1) * scale - 1),
(intensity, intensity, intensity), cv.CV_FILLED)
print intensity
cv.ShowImage("src", src)
cv.ShowImage("hsv", hist_img)
cv.WaitKey(0)
else:
print "usage : python example_7_1.py <image>"
def detect_and_send(img, cascade):
global screenW, screenH, threshold, Debug, showCamera
global oldTimeDebug
global somme, cpt
# allocate temporary images
gray = cv.CreateImage((img.width, img.height), 8, 1)
small_img = cv.CreateImage((cv.Round(
img.width / image_scale), cv.Round(img.height / image_scale)), 8, 1)
# l'algo de reconnaissance de forme est basé sur des images en niveaux de gris
cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
# Réduit la taille de l'image pour un traitement plus rapide
cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
#Augmente le contraste (égalise la luminosité)
cv.EqualizeHist(small_img, small_img)
centreCercle = (0, 0)
if (cascade):
t = cv.GetTickCount()
faces = cv.HaarDetectObjects(small_img, cascade, cv.CreateMemStorage(0), \
haar_scale, min_neighbors, haar_flags, min_size)
t = cv.GetTickCount() - t
cpt += 1
somme += t / (cv.GetTickFrequency() * 1000.)
if Debug == 1:
print "detection time = %gms" % (t /
(cv.GetTickFrequency() * 1000.))
if faces:
oldx = centreCercle[0]
oldy = centreCercle[1]
for ((x, y, w, h), n) in faces:
#la bounding box encadrant le visage, sachant que l'image a été remdimensionnée pour le traitement (+ rapide)
pt1 = (int(x * image_scale), int(y * image_scale))
pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
cv.Rectangle(img, pt1, pt2, (255, 0, 0))
#On doit calculer l'opposé de la valeur qu'on trouverait car la caméra n'agit pas comme un miroir
#en effet si je bouge vers la droite l'image fait bouger la tête vers la gauche (donc vers la droite selon mon
#point de vue)
ex = -(float(centreCercle[0]) / float(img.width) * screenW -
screenW / 2.0)
ey = -(
(float(centreCercle[1]) / float(img.height) * screenH -
screenH / 2.0)
) #+ screenH / 2.0 #compense le fait que la caméra est au dessus de l'écran
oldx = centreCercle[0]
oldy = centreCercle[1]
#Détermination de ez en fonction de la taille de la tête à l'écran
ez = -1.50567 * (h * image_scale) + 893.688
#assure la stabilité de l'image, si on enlève ce test ça tremblote à mort car la détection n'étant pas ultra précise
#la position de la tête est considérée comme changeante en permanence
#la constante a été déterminée empiriquement sur une webcam intégrée de mcbook 13,3"(voir fichier conf.cfg)
#Gestion du temps pour permettre de bouger lentement (fonctionne mal pour l'instant)
newTime = time.time()
if abs(oldx - centreCercle[0]) > threshold or abs(
oldy - centreCercle[1]
) > threshold: #or (newTime - oldTimeThr) > 1
liblo.send(target, "/coordonnees/", ex, ey, ez)
oldTimeThr = newTime
#DEBUG, affiche la position des yeux toutes les 1/2 secondes
if Debug == 1:
if (newTime - oldTimeDebug) > 0.5:
print "\n-----------DEBUG----------"
print "Coordonnée centre sur image : " + str(
centreCercle[0]) + " " + str(centreCercle[1])
print "Coordonnées yeux dans référentiel 3D : "
print "ex = " + str(ex) + " ey = " + str(
ey) + " ez = " + str(ez)
oldTimeDebug = newTime
#debug tracking visage
if showCamera == 1:
cv.ShowImage("result", img)
ser.write(str(targetx) + "x" + str(targety) + "y")
def directSentry(direction):
# 0: reset 1-4: N-W
ser.write(str(direction))
if __name__ == '__main__':
# activate camera
cv.NamedWindow('tempwindow', cv.CV_WINDOW_AUTOSIZE)
capture = cv.CaptureFromCAM(0)
image = cv.QueryFrame(capture)
W, H = cv.GetSize(image)
#calculate center of image
centerScreenX = cv.Round(W / 2)
centerScreenY = cv.Round(H / 2)
# initialize serial port
ser = serial.Serial(port='/dev/tty.usbmodemfd111',
baudrate=19200,
timeout=0)
directSentry(0)
#moveSentry(90,90)
while True:
image = cv.QueryFrame(capture)
face = detectFace(image)
if face:
((hx, hy, hw, hh), hn) = face
cv.SetImageROI(image, (int(hx), int(hy), hw, hh))
def sort_bins(self):
perimeter_errors = []
area_errors = []
# 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
perimeter_errors.append(confirmed)
continue
if line_distance(confirmed.corner1, confirmed.corner2) * line_distance(confirmed.corner1, confirmed.corner3) > self.area_thresh or \
line_distance(confirmed.corner4, confirmed.corner2) * line_distance(confirmed.corner4, confirmed.corner3) > self.area_thresh:
print "area error"
confirmed.last_seen -= 5
area_errors.append(confirmed)
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)
for error in perimeter_errors:
cv.Circle(self.debug_frame, (int(error.midx), int(error.midy)), 15, (0, 255, 255), 2, 8, 0)
for error in area_errors:
cv.Circle(self.debug_frame, (int(error.midx), int(error.midy)), 15, (255, 0, 255), 2, 8, 0)
def find_lines(frame):
# Resize to 640x480
frame_size = cv.GetSize(frame)
if frame_size[0] != 640:
frame_small = cv.CreateMat(480, 640, cv.CV_8UC3)
cv.Resize(frame, frame_small)
else:
frame_small = frame
# Threshold by distance: blank out all top pixels
cv.Rectangle(frame_small, (0, 0), (640, 80), (0, 0, 0, 0), cv.CV_FILLED)
# Convert to grayscale
frame_size = cv.GetSize(frame_small)
frame_gray = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.CvtColor(frame_small, frame_gray, cv.CV_BGR2GRAY)
# Use color thresholding to get white lines
threshold = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.Threshold(frame_gray, threshold, 190, 255, cv.CV_THRESH_BINARY)
# Morphological ops to reduce noise
# TODO try to reduce sizes to increase detection of faraway lines
openElement = cv.CreateStructuringElementEx(7, 7, 3, 3, cv.CV_SHAPE_RECT)
closeElement = cv.CreateStructuringElementEx(11, 11, 5, 5,
cv.CV_SHAPE_RECT)
cvOpen(threshold, threshold, openElement)
cvClose(threshold, threshold, closeElement)
# Use Canny edge detection to find edges
edges = cv.CreateImage(frame_size, cv.IPL_DEPTH_8U, 1)
cv.Canny(threshold, edges, 100, 200)
# Use Hough transform to find equations for lines
line_storage = cv.CreateMemStorage()
lines = cv.HoughLines2(edges, line_storage, cv.CV_HOUGH_STANDARD, 1,
cv.CV_PI / 180.0, 120)
lines = list(lines)
# Remove spurious line from the black rectangle up top
for line in lines[:]:
if (abs(180 - line[0]) < 10
and abs(util.normalizeRadians(cv.CV_PI / 2 - line[1])) < 0.01):
lines.remove(line)
# Group lines that are within r +/-12 and theta +/- 5 degrees
grouped_lines = []
r_threshold = 12 # in px
theta_threshold = cv.CV_PI * 5 / 180 # in radians
while len(lines) > 0:
line1 = normalizeLine(lines.pop())
avg_line = line1
matched_lines = [line1]
for j in range(len(lines) - 1, -1, -1):
line2 = normalizeLine(lines[j])
if verbose:
# Print which criteria were matched
if (abs(avg_line[0] - line2[0]) < r_threshold):
print 1,
if (abs(util.normalizeRadians(avg_line[1] - line2[1])) <
theta_threshold):
print 2,
print avg_line, line2
if (abs(avg_line[0] - line2[0]) < r_threshold and
abs(util.normalizeRadians(avg_line[1] - line2[1])) < \
theta_threshold):
matched_lines.append(line2)
avg_line = avgLines(matched_lines)
lines.pop(j)
if verbose: print matched_lines
grouped_lines.append(avg_line)
lines = grouped_lines
# Group possible pairs of lines by smallest angle difference
grouped_lines = []
while len(lines) > 0:
line1 = normalizeLine(lines.pop())
closest = None
for j in range(len(lines) - 1, -1, -1):
line2 = normalizeLine(lines[j])
# Find the closest match
if ((closest is None
or abs(util.normalizeRadians(line1[1] - line2[1])) < \
abs(util.normalizeRadians(line1[1] - closest[1])))
# Make sure difference < pi/4 to reduce errors
and abs(util.normalizeRadians(line1[1] - line2[1])) < \
cv.CV_PI / 4):
closest = line2
if closest is not None:
lines.remove(closest)
# Sort list by line[0] (radius)
if line1[0] > closest[0]:
line1, closest = closest, line1
# Make a tuple (line1, line2) or (line,) if no match found
grouped_lines.append((line1, closest))
else:
grouped_lines.append((line1, ))
# Print lines
if len(grouped_lines) > 0:
print 'Groups of lines:', grouped_lines
i = 0
for group in grouped_lines:
for j in range(len(group)):
rho, theta = group[j]
a, b = np.cos(theta), np.sin(theta)
x0, y0 = a * rho, b * rho
pt1 = (cv.Round(x0 + 1000 * (-b)), cv.Round(y0 + 1000 * (a)))
pt2 = (cv.Round(x0 - 1000 * (-b)), cv.Round(y0 - 1000 * (a)))
#cv.Line(frame_small, pt1, pt2,
# hv2rgb(360.0*i/len(grouped_lines), 1.0), 1, 8)
i += 1
# If 2+ groups of lines, find corners (intersection point of lines)
intersection_pts = []
if len(grouped_lines) > 1:
for i in range(len(grouped_lines)):
pair1 = grouped_lines[i]
for j in range(i + 1, len(grouped_lines)):
pair2 = grouped_lines[j]
# Make sure their angles differ by more than 10 deg to
# reduce errors
if (abs(util.normalizeRadians(pair1[0][1] - pair2[0][1])) <
cv.CV_PI * 10 / 180):
break
# Enumerate intersection points
pts = []
for line1 in pair1:
for line2 in pair2:
pts.append(lineIntersection(line1, line2))
# Find average of intersection points
x = sum(pt[0] for pt in pts) / len(pts)
y = sum(pt[1] for pt in pts) / len(pts)
pt = (x, y)
print 'Intersection:', pt,
intersection_pts.append(pt)
pt = (cv.Round(x), cv.Round(y))
cv.Circle(frame_small, pt, 4, (0, 255, 0, 0))
# Find direction of intersection by following each line
# (direction is defined as the point of the T)
angles = []
for pair in grouped_lines:
angles.append(pair[0][1] + cv.CV_PI / 2)
angles.append(pair[0][1] - cv.CV_PI / 2)
for angle in angles:
# Look 50px down the line for white pixels
# TODO look a variable amount
x1 = x + 50 * np.cos(angle)
y1 = y + 50 * np.sin(angle)
# Enforce limits
# TODO handle when intersection is off the bounds of the image
# Currently orientation of the intersection is not being used
# by the particle filter
x1 = min(max(0, x1), frame_size[0] - 1)
y1 = min(max(0, y1), frame_size[1] - 1)
srchPt = cv.Round(x1), cv.Round(y1)
if threshold[srchPt[1], srchPt[0]] == 0:
x1 = x + 50 * np.cos(angle + cv.CV_PI)
y1 = y + 50 * np.sin(angle + cv.CV_PI)
invSrchPt = cv.Round(x1), cv.Round(y1)
cv.Line(frame_small, pt, invSrchPt, (0, 255, 0, 0), 1,
8)
print 'Angle:', angle + cv.CV_PI
break
# Convert line equations into line segments
line_segments = []
for group in grouped_lines:
if len(group) == 2:
# Get the average of the lines in a pair
line1, line2 = group
line = ((line1[0] + line2[0]) / 2.0, (line1[1] + line2[1]) / 2.0)
else:
line = group[0]
# Look down the line for the endpoints of the white region
line_segment = lineToVisibleSegment(line, threshold)
if line_segment != None:
line_segments.append(line_segment)
print 'Line segments:', line_segments
i = 0
for pt1, pt2 in line_segments:
pt1 = cv.Round(pt1[0]), cv.Round(pt1[1])
pt2 = cv.Round(pt2[0]), cv.Round(pt2[1])
cv.Line(frame_small, pt1, pt2,
hv2rgb(360.0 * i / len(line_segments), 1.0), 2, 8)
i += 1
cv.ShowImage('frame', frame_small)
cv.ShowImage('edges', threshold)
return line_segments, intersection_pts
