def image_callback(data):
global running
if (running):
image = bridge.imgmsg_to_cv(data, "bgr8")
#normalize image
cv.Split(image, rgb_r, rgb_g, rgb_b, None)
red_mean = cv2.mean(np.asarray(rgb_r[:, :]))
cv.Div(src2=cv.fromarray(np.ones((480, 640))),
src1=rgb_r,
dst=scaled_r,
scale=128 / red_mean[0])
green_mean = cv2.mean(np.asarray(rgb_g[:, :]))
cv.Div(src2=cv.fromarray(np.ones((480, 640))),
src1=rgb_g,
dst=scaled_g,
scale=128 / green_mean[0])
blue_mean = cv2.mean(np.asarray(rgb_b[:, :]))
cv.Div(src2=cv.fromarray(np.ones((480, 640))),
src1=rgb_b,
dst=scaled_b,
scale=128 / blue_mean[0])
cv.Merge(scaled_r, scaled_g, scaled_b, None, cv_image)
cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV) # --convert from BGR to HSV
cv.CvtColor(cv_image, lab, cv.CV_BGR2Lab)
cv.Split(hsv, hsv_h, hsv_s, hsv_v, None)
cv.Split(cv_image, rgb_r, rgb_g, rgb_b, None)
cv.Split(lab, lab_l, lab_a, lab_b, None)
cv.Split(luv, luv_l, luv_u, luv_v, None)
cv.Split(hls, hls_h, hls_l, hls_s, None)
cv.Split(xyz, xyz_x, xyz_y, xyz_x, None)
cv.Split(ycrcb, ycrcb_y, ycrcb_cr, ycrcb_cb, None)
cv.Not(lab_a, a_not)
cv.Sub(hsv_s, a_not, sa)
cv.Sub(luv_u, hls_h, test)
cv.Sub(hls_s, hls_h, sminh)
threshold_red(sa)
cv.ShowImage("red", red_dilated_image)
red_contours, _ = cv2.findContours(image=np.asarray(
red_dilated_image[:, :]),
mode=cv.CV_RETR_EXTERNAL,
method=cv.CV_CHAIN_APPROX_SIMPLE)
print_lidar_projections(cv_image)
circles = extract_circles(red_contours, [1, 0, 0])
for x, y, radius in circles:
cv.Circle(cv_image, (x, y), radius, [0, 0, 1], 3)
cv.SetMouseCallback("camera feed", mouse_callback, hsv_image)
cv.ShowImage("camera feed", cv_image)
cv.WaitKey(3)
def compare_imgs(self, img1, img2):
back_proj_img, hist1 = self.back_project_hs(img1)
back_proj_img2, hist2 = self.back_project_hs(img2)
scratch = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
scratch2 = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
cv.Zero(scratch)
cv.Zero(scratch2)
#cv.Sub(back_proj_img, back_proj_img2, scratch2) #opposite noise, but excludes object
cv.Sub(back_proj_img2, back_proj_img, scratch2) #noise, but includes object if failed,
cv.Sub(scratch2, ha.avg_noise, scratch)
return scratch
def getMaskedImage(frame, mask):
#cv.Not(myMask, myMask)
rgbImg = cv.CreateImage(cv.GetSize(frame), 8, 3)
resImg = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.Sub(frame, frame, rgbImg, mask)
cv.CvtColor(rgbImg, resImg, cv.CV_BGR2GRAY)
return (resImg)
def getIris(frame):
iris = []
copyImg = cv.CloneImage(frame)
resImg = cv.CloneImage(frame)
grayImg = cv.CreateImage(cv.GetSize(frame), 8, 1)
mask = cv.CreateImage(cv.GetSize(frame), 8, 1)
storage = cv.CreateMat(frame.width, 1, cv.CV_32FC3)
cv.CvtColor(frame, grayImg, cv.CV_BGR2GRAY)
cv.Canny(grayImg, grayImg, 5, 70, 3)
cv.Smooth(grayImg, grayImg, cv.CV_GAUSSIAN, 7, 7)
circles = getCircles(grayImg)
iris.append(resImg)
for circle in circles:
rad = int(circle[0][2])
global radius
radius = rad
cv.Circle(mask, centroid, rad, cv.CV_RGB(255, 255, 255), cv.CV_FILLED)
cv.Not(mask, mask)
cv.Sub(frame, copyImg, resImg, mask)
x = int(centroid[0] - rad)
y = int(centroid[1] - rad)
w = int(rad * 2)
h = w
cv.SetImageROI(resImg, (x, y, w, h))
cropImg = cv.CreateImage((w, h), 8, 3)
cv.Copy(resImg, cropImg)
cv.ResetImageROI(resImg)
return (cropImg)
return (resImg)
def avgstd_image_list(images):
mean = None
std = None
if len(images) > 0:
scale = 1. / len(images)
mean = cv.CreateImage(cv.GetSize(images[0]), cv.IPL_DEPTH_32F,
images[0].channels)
std = cv.CreateImage(cv.GetSize(images[0]), cv.IPL_DEPTH_32F,
images[0].channels)
buf = cv.CreateImage(cv.GetSize(images[0]), cv.IPL_DEPTH_32F,
images[0].channels)
for image in images:
cv.Add(image, mean, mean)
cv.Mul(image, image, buf)
cv.Add(buf, std, std)
cv.ConvertScale(mean, mean, scale)
cv.ConvertScale(std, std, scale)
cv.Mul(mean, mean, buf)
cv.Sub(std, buf, std)
cv.Pow(std, std, 0.5)
meanresult = cv.CreateImage(cv.GetSize(images[0]), images[0].depth,
images[0].channels)
stdresult = cv.CreateImage(cv.GetSize(images[0]), images[0].depth,
images[0].channels)
cv.ConvertScale(mean, meanresult)
cv.ConvertScale(std, stdresult)
del buf
del std
del mean
return (meanresult, stdresult)
def first_bigger_then_second(a, b):
sub = image_empty_clone(a)
cv.Sub(a, b, sub) #Thouse with R < G will become 0
binary = image_empty_clone(a)
#Make binary image
cv.Threshold(sub, binary, 1, 255, cv.CV_THRESH_BINARY)
return binary
def subtract(self, thres_chan):
cv.RunningAvg(thres_chan, self.accumulator, self.adaptation_rate)
cv.CvtScale(thres_chan, self.green32_img)
cv.Sub(self.green32_img, self.accumulator, self.difference_img)
cv.Threshold(self.difference_img, self.thresholded_img, self.threshold,
1, cv.CV_THRESH_BINARY)
cv.Dilate(self.thresholded_img, self.thresholded_img, iterations=1)
blob.remove_large_blobs(self.thresholded_img, max_area=self.max_area)
return self.thresholded_img
def sub_image(self, imagecurr, imageprev, divid=True):
imagesize = (imagecurr.width, imagecurr.height)
image = cv.CreateImage(imagesize, cv.IPL_DEPTH_8U, 1)
cv.Sub(imagecurr, imageprev, image)
# use pyramid/cone to ponderate the weight
# ie. moves in corners are more important than in the center
if divid:
cv.Div(image, self.cone, image)
cv.Flip(image, flipMode=1) # for webcam
return image
def sub_intersection(amap, apoly, maxwidth, maxheight):
polymap = cv.CreateImage((maxwidth,
maxheight),
cv.IPL_DEPTH_8U,1)
cv.FillPoly(polymap, [apoly], im.color.blue)
intersection = cv.CreateImage((maxwidth,
maxheight),
cv.IPL_DEPTH_8U,1)
cv.And(polymap, amap, intersection)
cv.Sub(amap, intersection, amap)
def ssdScore(f1, f2):
global size #size o f SSD window
#subtracts f2 from f1
sub_f1_f2 = cv.CreateMat(size, size, cv.CV_64FC1)
cv.Sub(f1, f2, sub_f1_f2)
#square and add
f1_f2_square = cv.CreateMat(size, size, cv.CV_64FC1)
cv.Pow(sub_f1_f2, f1_f2_square, 2)
score = cv.Sum(f1_f2_square)
return score[0] / (size * size)
def repeat():
global CAM_CFG, lastframe, diffframe, camframe, camhist, camhist_img
frame = cv.GetSubRect(cv.QueryFrame(CAM_CFG['handler']), CAM_CFG['roi'])
# This takes 1% CPU:
#framearr = cv2array(frame)
# This takes 7% CPU:
#framearr = framearr.astype(np.float64)*1.0/256
# This takes 3% CPU:
cv.ConvertScale(frame, camframe, scale=1.0 / 256)
# This takes 2% CPU:
#camframearr = cv2array(camframe)
# Calculate (cam-dark)/flat. Without mask might be faster sometimes.
cv.Sub(camframe, darkframe, camframe, mask=CAM_CFG['mask'])
cv.Div(camframe, flatframe, camframe, 1)
# # Calculate cam - last
cv.Sub(camframe, lastframe, diffframe, mask=CAM_CFG['mask'])
# Make histogram of camframe
camhist, camhist_img = calc_1dhisto(camframe,
nbin=ibins,
scale=scale,
histh=histh,
hist=camhist,
histimg=camhist_img)
if (LIVE):
cv.ShowImage("cam_live", camframe)
cv.ShowImage("cam_other", diffframe)
cv.ShowImage("cam_histo", camhist_img)
c = cv.WaitKey(10)
if (c == "n"
): #in "n" key is pressed while the popup window is in focus
pass
CAM_CFG['buf'][(CAM_CFG['frameidx']) % len(CAM_CFG['buf'])] = camframe
CAM_CFG['frameidx'] += 1
lastframe = cv.CloneImage(camframe)
def precornerdetect(image):
# assume that the image is floating-point
corners = cv.CloneMat(image)
cv.PreCornerDetect(image, corners, 3)
dilated_corners = cv.CloneMat(image)
cv.Dilate(corners, dilated_corners, None, 1)
corner_mask = cv.CreateMat(image.rows, image.cols, cv.CV_8UC1)
cv.Sub(corners, dilated_corners, corners)
cv.CmpS(corners, 0, corner_mask, cv.CV_CMP_GE)
return (corners, corner_mask)
def get_circular_roi(self, center, rad, src):
# create an empty one-channel image
outside = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_8U, 1)
cv.SetZero(outside) # make sure nothing is in the image
# create white (filled) circle, by passing -1
cv.Circle(outside, center, rad, ColorSpace.RGB_WHITE, -1)
# invert to create the circular window
cv.Not(outside, outside)
# Subtract the background from image to get only the are desired ROI
roi = cv.CreateImage(cv.GetSize(src), cv.IPL_DEPTH_8U, 1)
cv.Sub(src, outside, roi)
return roi
def remove_background(self, frame):
"""Remove background, leaving robots and some noise.
It is not safe to modify the returned image, as it will be
re-initialised each time preprocess is run.
"""
logging.debug("Performing background subtraction")
#cv.CvtColor(frame, self.Igray, cv.CV_BGR2GRAY)
cv.Sub(frame, self.bg, self.Imask)
return self.Imask
def crunch(): size = cv.GetSize(band3) assert size == cv.GetSize(band4) numerator = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Sub(band4, band3, numerator) denominator = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Add(band4, band3, denominator) ndvi_img = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Div(numerator, denominator, ndvi_img) # (NDVI + 1) cv.AddS(ndvi_img, 1, ndvi_img) return ndvi_img
def pixel_distance_matrix(images):
buf = cv.CreateImage(cv.GetSize(images[0]), images[0].depth,
images[0].channels)
distances = np.zeros((len(images), len(images)))
for i in xrange(len(images)):
for j in xrange(i + 1, len(images)):
cv.Sub(images[i], images[j], buf)
cv.Pow(buf, buf, 2)
distance = cv.Sum(buf)[0]
distances[i, j] = distance
distances[j, i] = distance
del buf
return distances
def max_contrast(image):
"""Maximise the contrast of an image using top and bottom hat filters."""
size = cv.GetSize(image)
bh = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
th = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
s1 = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
s2 = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
el = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_ELLIPSE)
cv.MorphologyEx(image, th, None, el, cv.CV_MOP_TOPHAT, 1)
cv.MorphologyEx(image, bh, None, el, cv.CV_MOP_BLACKHAT, 1)
cv.Add(image, th, s1)
cv.Sub(s1, bh, s2)
return s2
def calc_stats(self):
cv.NamedWindow("noise", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("img1_back", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("img2_back", cv.CV_WINDOW_AUTOSIZE)
self.check_for_hist()
self.avg_noise = cv.CreateImage(cv.GetSize(self.background_noise[0]),
8, 1)
cv.Zero(self.avg_noise)
for i in xrange(len(self.background_noise) - 1):
cv.ShowImage("noise", self.avg_noise)
back_proj_img1, hist1 = self.back_project_hs(
self.background_noise[i])
back_proj_img2, hist2 = self.back_project_hs(
self.background_noise[i + 1])
self.accumulateBackground(back_proj_img1)
cv.ShowImage("img1_back", back_proj_img1)
cv.ShowImage("img2_back", back_proj_img2)
scratch = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
scratch2 = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
# do something clever with ands ors and diffs
cv.Zero(scratch)
cv.Zero(scratch2)
cv.Sub(back_proj_img2, back_proj_img1,
scratch2) #noise, but includes object if failed,
#cv.Sub(scratch2, self.avg_noise, scratch)
#cv.Or(self.avg_noise, scratch2, self.avg_noise)
cv.Or(self.avg_noise, scratch2, self.avg_noise)
cv.ShowImage("diff_back", scratch2)
cv.ShowImage("diff_noise_scratch", scratch)
cv.WaitKey(-1)
self.createModelsfromStats()
print self.Icount
cv.NamedWindow("Ilow", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("Ihi", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow("IavgF", cv.CV_WINDOW_AUTOSIZE)
cv.ShowImage("Ihi", self.IhiF)
cv.ShowImage("Ilow", self.IlowF)
cv.ShowImage("IavgF", self.IavgF)
def rgb_min_max_diff_plane(r, g, b, level):
rg_max = image_empty_clone(r)
cv.Max(r, g, rg_max)
rgb_max = image_empty_clone(b)
cv.Max(rg_max, b, rgb_max)
rg_min = image_empty_clone(r)
cv.Min(r, g, rg_min)
rgb_min = image_empty_clone(b)
cv.Min(rg_min, b, rgb_min)
rgb_sub = image_empty_clone(rgb_max)
cv.Sub(rgb_max, rgb_min, rgb_sub)
binary = image_empty_clone(r)
cv.Threshold(rgb_sub, binary, level, 255, cv.CV_THRESH_BINARY)
return binary
def doloop():
global depth, rgb
for i in range(1,10):
(depth,_), (rgb,_) = get_depth(), get_video()
bg=cv.CloneMat(cv.fromarray(depth.astype(numpy.uint8)))
scratch = cv.CreateImage((640,480),8,1)
scratch2 = cv.CreateImage((640,480),8,1)
cv.SaveImage('bg.png',bg)xz
while True:
# Get a fresh frame
(depth,_), (rgb,_) = get_depth(), get_video()
depth=cv.fromarray(depth.astype(numpy.uint8))
cv.AbsDiff(bg,depth,scratch)
cv.Sub(scratch,2,10,scratch2)
# cv.ConvertScale(scratch,scratch2,50)
cv.Add(depth,scratch2,scratch2)
# Simple Downsample
cv.ShowImage('both',scratch2)
cv.WaitKey(10)
def measure(im, debug=False):
gray = image.rgb2gray(im)
size = cv.GetSize(im)
total = float(size[0] * size[1])
edges = image.auto_edges(im)
hue, sat, val = tuple(
map(image.equalize_hist, image.split(image.rgb2hsv(im))))
l, u, v = tuple(map(image.equalize_hist, image.split(image.rgb2luv(im))))
values = []
if debug:
image.show(l, "L")
image.show(val, "Value")
sat = image.threshold(val, 255 - 32) #image.And(val, sat)
if debug:
image.show(sat, "Thresh")
#cv.And(val, l, val)
cv.Sub(l, sat, l)
cv.Set(l, 0, image.dilate(edges, iterations=3))
if debug:
image.show(l, "L - Value")
val = l
g = Grid(cv.GetSize(val))
images = g.split_into(val, 16)
arr = image.cv2array(val)
avgmean, avgstd = arr.mean(), arr.std()
for i in images:
a = image.cv2array(i)
mean, std = abs(a.mean() - avgmean), max(a.std(), 0)
values.append((mean + std))
if debug:
print values
print "AVG", avgmean, avgstd
image.show(val, "Result")
return val, (avgmean, avgstd, len([v for v in values if v > avgstd * 2]))
def capture(self):
blank = self.get_picture_of_projection(self.blank_projection)
positive = self.get_picture_of_projection(
self.positive_chessboard_projection)
negative = self.get_picture_of_projection(
self.negative_chessboard_projection)
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positive, negative, difference)
cv.NamedWindow("blank", flags=0)
cv.ShowImage("blank", blank)
cv.WaitKey(300)
cv.NamedWindow("difference", flags=0)
cv.ShowImage("difference", difference)
cv.WaitKey(300)
camera_image_points, camera_object_points = detect_chessboard(
blank, self.printed_chessboard_corners_x,
self.printed_chessboard_corners_y, self.printed_chessboard_spacing,
self.printed_chessboard_spacing)
if camera_image_points is None:
return False
cv.UndistortPoints(camera_image_points, camera_image_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
homography = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(camera_image_points, camera_object_points,
homography)
object_points, dummy = detect_chessboard(
difference, self.projected_chessboard_corners_x,
self.projected_chessboard_corners_y, None, None)
if object_points is None:
return False
cv.UndistortPoints(object_points, object_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
cv.PerspectiveTransform(object_points, object_points, homography)
object_points_3d = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC3)
x = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
y = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.Split(object_points, x, y, None, None)
z = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.SetZero(z)
cv.Merge(x, y, z, None, object_points_3d)
self.object_points.append(object_points_3d)
self.image_points.append(self.get_scene_image_points())
self.number_of_scenes += 1
return True
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()
# -*- coding: utf-8 -*-
"""
Created on Tue May 22 16:41:21 2012
@author: IntelligentSystems
"""
import cv
import os
import glob
count = 0
average = None
path = 'Clips/'
for infile in glob.glob(os.path.join(path, '*.bmp')):
image = cv.LoadImage(infile, False)
if average is None:
average = cv.CreateImage(cv.GetSize(image), 32, 1)
image_aux = cv.CloneImage(average)
cv.Convert(image, average)
else:
count = count + 1
scale = 1 / count
cv.Convert(image, image_aux)
cv.Sub(image_aux, average, image_aux)
cv.Scale(image_aux, image_aux, scale)
cv.Add(average, image_aux, average)
cv.Convert(average, image)
cv.SaveImage('background.bmp', image)
def DetectaSombra(frame, bg):
dbg = 1
if dbg:
t1 = time.time()
print 'Detectando sombras na imagem...'
# gera as imagens de cada canal RGB
imgCinza = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
imgHSV = cv.CloneImage(frame)
imgH = cv.CloneImage(imgCinza)
imgS = cv.CloneImage(imgCinza)
imgV = cv.CloneImage(imgCinza)
imgR = cv.CloneImage(imgCinza)
imgG = cv.CloneImage(imgCinza)
imgB = cv.CloneImage(imgCinza)
bgCinza = cv.CreateImage(cv.GetSize(bg), cv.IPL_DEPTH_8U, 1)
bgHSV = cv.CloneImage(bg)
bgH = cv.CloneImage(bgCinza)
bgS = cv.CloneImage(bgCinza)
bgV = cv.CloneImage(bgCinza)
bgR = cv.CloneImage(bgCinza)
bgG = cv.CloneImage(bgCinza)
bgB = cv.CloneImage(bgCinza)
# gera as imagens de cada frame e backgroun nos canais de HSV e RGB
cv.CvtColor(frame, imgHSV, cv.CV_BGR2HSV)
cv.Split(imgHSV, imgH, imgS, imgV, None)
cv.Split(frame, imgR, imgG, imgB, None)
cv.CvtColor(bg, bgHSV, cv.CV_BGR2HSV)
cv.Split(bgHSV, bgH, bgS, bgV, None)
cv.Split(bg, bgR, bgG, bgB, None)
# inicio de calculos para descobrir sombras.
ivbv = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
cv.Div(imgV, bgV, ivbv, 255)
isbs = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
cv.Sub(imgS, bgS, isbs)
ihbh = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
cv.AbsDiff(imgH, bgH, ihbh)
# parametros de deteccao de sombra
alfa = 190
beta = 210
thrSat = 20
thrHue = 50
alfa = 220
beta = 240
thrSat = 90
thrHue = 90
nErode = 0
nDilate = 0
# trata ivbv
imgThr_ivbv = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
# deixa apenas os menores que beta
cv.Threshold(ivbv, imgThr_ivbv, beta, 255, cv.CV_THRESH_TRUNC)
# deixa apenas os maiores que alfa
cv.Threshold(imgThr_ivbv, imgThr_ivbv, alfa, 255, cv.CV_THRESH_TOZERO)
# binariza
cv.Threshold(imgThr_ivbv, imgThr_ivbv, alfa, 255, cv.CV_THRESH_BINARY)
# trata isbs
imgThr_isbs = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
# deixa apenas os menores que thrSat
cv.Threshold(isbs, imgThr_isbs, thrSat, 255, cv.CV_THRESH_BINARY)
# trata isbs
imgThr_ihbh = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
# deixa apenas os menores que thrSat
cv.Threshold(ihbh, imgThr_ihbh, thrHue, 255, cv.CV_THRESH_BINARY_INV)
# onde é preto em todas as imagens, é sombra
imgSombra = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
cv.Not(imgThr_ivbv, imgThr_ivbv)
cv.Not(imgThr_isbs, imgThr_isbs)
cv.And(imgThr_ivbv, imgThr_isbs, imgSombra)
cv.Not(imgThr_ihbh, imgThr_ihbh)
cv.And(imgSombra, imgThr_ihbh, imgSombra)
for i in range(nErode):
cv.Erode(imgSombra, imgSombra)
for i in range(nDilate):
cv.Dilate(imgSombra, imgSombra)
if dbg:
print 'Tempo para detectar sombras: %.5f' % (time.time() - t1)
#exibe frames de saida
#destaca de verde a sombra sobre o frame
frameDestacado = cv.CloneImage(frame)
cv.Or(imgG, imgSombra, imgG)
cv.Merge(imgR, imgG, imgB, None, frameDestacado)
'''
cv.ShowImage('frameDestacado',frameDestacado)
cv.WaitKey()
'''
retorno = {}
retorno['sombra'] = imgSombra
retorno['sombraDestacada'] = frameDestacado
return retorno
cv.ShowImage('ivbv', ivbv)
cv.ShowImage('isbs', isbs)
cv.ShowImage('ihbh', ihbh)
cv.ShowImage('imgThr_isbs', imgThr_isbs)
cv.ShowImage('imgThr_ivbv', imgThr_ivbv)
cv.ShowImage('imgThr_ihbh', imgThr_ihbh)
cv.ShowImage('imgSombra', imgSombra)
cv.WaitKey()
sys.exit()
frameMerge = cv.CloneImage(frame)
cv.Merge(imgR, imgR, imgR, None, frameMerge)
cv.ShowImage('frame', frame)
cv.ShowImage('frameMerge', frameMerge)
cv.ShowImage('imgR', imgR)
cv.ShowImage('imgG', imgG)
cv.ShowImage('imgB', imgB)
cv.ShowImage('imgH', imgH)
cv.ShowImage('imgS', imgS)
cv.ShowImage('imgV', imgV)
cv.WaitKey()
return 0
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()
def get_projector_line_associations(self):
rospy.loginfo("Scanning...")
positives = []
negatives = []
for i in range(self.number_of_projection_patterns):
positives.append(
self.get_picture_of_projection(
self.predistorted_positive_projections[i]))
negatives.append(
self.get_picture_of_projection(
self.predistorted_negative_projections[i]))
rospy.loginfo("Thresholding...")
strike_sum = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.SetZero(strike_sum)
gray_codes = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.SetZero(gray_codes)
for i in range(self.number_of_projection_patterns):
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positives[i], negatives[i], difference)
absolute_difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_8UC1)
cv.AbsDiff(positives[i], negatives[i], absolute_difference)
#Mark all the pixels that were "too close to call" and add them to the running total
strike_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(absolute_difference, self.threshold, strike_mask,
cv.CV_CMP_LT)
strikes = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.Set(strikes, 1, strike_mask)
cv.Add(strikes, strike_sum, strike_sum)
#Set the corresponding bit in the gray_code
bit_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(difference, 0, bit_mask, cv.CV_CMP_GT)
bit_values = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.Set(bit_values, 2**i, bit_mask)
cv.Or(bit_values, gray_codes, gray_codes)
rospy.loginfo("Decoding...")
# Decode every gray code into binary
projector_line_associations = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
cv.Copy(gray_codes, projector_line_associations)
for i in range(
cv.CV_MAT_DEPTH(cv.GetElemType(projector_line_associations)),
-1, -1):
projector_line_associations_bitshifted_right = cv.CreateMat(
self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
#Using ConvertScale with a shift of -0.5 to do integer division for bitshifting right
cv.ConvertScale(projector_line_associations,
projector_line_associations_bitshifted_right,
(2**-(2**i)), -0.5)
cv.Xor(projector_line_associations,
projector_line_associations_bitshifted_right,
projector_line_associations)
rospy.loginfo("Post processing...")
# Remove all pixels that were "too close to call" more than once
strikeout_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(strike_sum, 1, strikeout_mask, cv.CV_CMP_GT)
cv.Set(projector_line_associations, -1, strikeout_mask)
# Remove all pixels that don't decode to a valid scanline number
invalid_scanline_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_8UC1)
cv.InRangeS(projector_line_associations, 0, self.number_of_scanlines,
invalid_scanline_mask)
cv.Not(invalid_scanline_mask, invalid_scanline_mask)
cv.Set(projector_line_associations, -1, invalid_scanline_mask)
self.display_scanline_associations(projector_line_associations)
return projector_line_associations
def sum_squared(img1, img2):
tmp = cv.CreateImage(cv.GetSize(img1), 8, 1)
cv.Sub(img1, img2, tmp)
cv.Pow(tmp, tmp, 2.0)
return cv.Sum(tmp)[0]
def find(self, img):
started = time.time()
gray = self.Cached('gray', img.height, img.width, cv.CV_8UC1)
cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
sobel = self.Cached('sobel', img.height, img.width, cv.CV_16SC1)
sobely = self.Cached('sobely', img.height, img.width, cv.CV_16SC1)
cv.Sobel(gray, sobel, 1, 0)
cv.Sobel(gray, sobely, 0, 1)
cv.Add(sobel, sobely, sobel)
sobel8 = self.Cached('sobel8', sobel.height, sobel.width, cv.CV_8UC1)
absnorm8(sobel, sobel8)
cv.Threshold(sobel8, sobel8, 128.0, 255.0, cv.CV_THRESH_BINARY)
sobel_integral = self.Cached('sobel_integral', img.height + 1,
img.width + 1, cv.CV_32SC1)
cv.Integral(sobel8, sobel_integral)
d = 16
_x1y1 = cv.GetSubRect(
sobel_integral,
(0, 0, sobel_integral.cols - d, sobel_integral.rows - d))
_x1y2 = cv.GetSubRect(
sobel_integral,
(0, d, sobel_integral.cols - d, sobel_integral.rows - d))
_x2y1 = cv.GetSubRect(
sobel_integral,
(d, 0, sobel_integral.cols - d, sobel_integral.rows - d))
_x2y2 = cv.GetSubRect(
sobel_integral,
(d, d, sobel_integral.cols - d, sobel_integral.rows - d))
summation = cv.CloneMat(_x2y2)
cv.Sub(summation, _x1y2, summation)
cv.Sub(summation, _x2y1, summation)
cv.Add(summation, _x1y1, summation)
sum8 = self.Cached('sum8', summation.height, summation.width,
cv.CV_8UC1)
absnorm8(summation, sum8)
cv.Threshold(sum8, sum8, 32.0, 255.0, cv.CV_THRESH_BINARY)
cv.ShowImage("sum8", sum8)
seq = cv.FindContours(sum8, cv.CreateMemStorage(), cv.CV_RETR_EXTERNAL)
subimg = cv.GetSubRect(img, (d / 2, d / 2, sum8.cols, sum8.rows))
t_cull = time.time() - started
seqs = []
while seq:
seqs.append(seq)
seq = seq.h_next()
started = time.time()
found = {}
print 'seqs', len(seqs)
for seq in seqs:
area = cv.ContourArea(seq)
if area > 1000:
rect = cv.BoundingRect(seq)
edge = int((14 / 14.) * math.sqrt(area) / 2 + 0.5)
candidate = cv.GetSubRect(subimg, rect)
sym = self.dm.decode(
candidate.width,
candidate.height,
buffer(candidate.tostring()),
max_count=1,
#min_edge = 6,
#max_edge = int(edge) # Units of 2 pixels
)
if sym:
onscreen = [(d / 2 + rect[0] + x, d / 2 + rect[1] + y)
for (x, y) in self.dm.stats(1)[1]]
found[sym] = onscreen
else:
print "FAILED"
t_brute = time.time() - started
print "cull took", t_cull, "brute", t_brute
return found
scratch = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
scratch2 = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
# do something clever with ands ors and diffs
cv.Zero(scratch)
cv.Zero(scratch2)
#idea is to have a background model from back_proj_img2, or at least an emtpy single shot
###cv.Sub(back_proj_img, back_proj_img2, scratch)
#cv.SubRS(back_proj_img, 255, scratch)
###cv.SubRS(back_proj_img2, 255, scratch2)
#cv.Sub(back_proj_img, back_proj_img2, scratch2) #opposite noise, but excludes object
cv.Sub(back_proj_img2, back_proj_img, scratch2) #noise, but includes object if failed,
#would need to learn before then update selectively
#Maybe want both added in the end.
cv.Sub(scratch2, avg_noise, scratch)
cv.Or(avg_noise, scratch2, avg_noise)
##adding this part fills in wherever the object has been too, heatmaps?
#cv.Sub(back_proj_img2, back_proj_img, scratch)
#cv.Or(avg_noise, scratch, avg_noise)
#
#cv.Sub(back_proj_img2, avg_noise, back_proj_img2)
#cv.Sub(scratch,, back_proj_img2)
cv.ShowImage("final", scratch)
#cv.Sub(scratch, avg_noise, scratch2)
