def Process(image, pos_var, pos_w, pos_phase, pos_psi):
global kernel_size
if kernel_size % 2 == 0:
kernel_size += 1
kernel = cv.CreateMat(kernel_size, kernel_size, cv.CV_32FC1)
# kernelimg = cv.CreateImage((kernel_size,kernel_size),cv.IPL_DEPTH_32F,1)
# big_kernelimg = cv.CreateImage((kernel_size*20,kernel_size*20),cv.IPL_DEPTH_32F,1)
src = cv.CreateImage((image.width, image.height), cv.IPL_DEPTH_8U, 1)
src_f = cv.CreateImage((image.width, image.height), cv.IPL_DEPTH_32F, 1)
# src = image #cv.CvtColor(image,src,cv.CV_BGR2GRAY) #no conversion is needed
if cv.GetElemType(image) == cv.CV_8UC3:
cv.CvtColor(image, src, cv.CV_BGR2GRAY)
else:
src = image
cv.ConvertScale(src, src_f, 1.0 / 255, 0)
dest = cv.CloneImage(src_f)
dest_mag = cv.CloneImage(src_f)
var = pos_var / 10.0
w = pos_w / 10.0
phase = pos_phase * cv.CV_PI / 180.0
psi = cv.CV_PI * pos_psi / 180.0
cv.Zero(kernel)
for x in range(-kernel_size / 2 + 1, kernel_size / 2 + 1):
for y in range(-kernel_size / 2 + 1, kernel_size / 2 + 1):
kernel_val = math.exp(-(
(x * x) +
(y * y)) / (2 * var)) * math.cos(w * x * math.cos(phase) +
w * y * math.sin(phase) + psi)
cv.Set2D(kernel, y + kernel_size / 2, x + kernel_size / 2,
cv.Scalar(kernel_val))
# cv.Set2D(kernelimg,y+kernel_size/2,x+kernel_size/2,cv.Scalar(kernel_val/2+0.5))
cv.Filter2D(src_f, dest, kernel, (-1, -1))
# cv.Resize(kernelimg,big_kernelimg)
cv.Pow(dest, dest_mag, 2)
# return (dest_mag, big_kernelimg, dest)
return (dest_mag, dest)
# cv.ShowImage("Mag",dest_mag)
# cv.ShowImage("Kernel",big_kernelimg)
# cv.ShowImage("Process window",dest)
def imgCropper1(xmin, xmax, ymin, ymax, image_to_be_resized):
print type(image_to_be_resized)
image = np.asarray(image_to_be_resized[:, :])
print 'after conversion hihihihii', type(image)
b = cv.CreateMat(image_to_be_resized.shape[0],
image_to_be_resized.shape[1], cv.CV_8UC1)
b = cv.fromarray(image_to_be_resized)
# image_to_be_resized = cv.fromArray(image_to_be_resized)
cv.SaveImage('before_cropping.jpg', b)
img = cv2.imread('Z_Rotated_color_image.jpg')
crop_color_img = img[ymin:ymax, xmin:xmax] # changed here
cropppped = image[ymin:ymax, xmin:xmax] # changed here
crop_string = 'Z_Cropped_image'
crop_img_string = crop_string + '.png'
cv2.imwrite('Z_Cropped_color_image.png', crop_color_img)
cv2.imwrite('Z_Cropped_image.png', cropppped)
return crop_img_string, cropppped, crop_color_img
def cvShiftDFT(src_arr, dst_arr):
size = cv.GetSize(src_arr)
dst_size = cv.GetSize(dst_arr)
if dst_size != size:
cv.Error(cv.CV_StsUnmatchedSizes, "cv.ShiftDFT",
"Source and Destination arrays must have equal sizes",
__FILE__, __LINE__)
if (src_arr is dst_arr):
tmp = cv.CreateMat(size[1] / 2, size[0] / 2, cv.GetElemType(src_arr))
cx = size[0] / 2
cy = size[1] / 2 # image center
q1 = cv.GetSubRect(src_arr, (0, 0, cx, cy))
q2 = cv.GetSubRect(src_arr, (cx, 0, cx, cy))
q3 = cv.GetSubRect(src_arr, (cx, cy, cx, cy))
q4 = cv.GetSubRect(src_arr, (0, cy, cx, cy))
d1 = cv.GetSubRect(src_arr, (0, 0, cx, cy))
d2 = cv.GetSubRect(src_arr, (cx, 0, cx, cy))
d3 = cv.GetSubRect(src_arr, (cx, cy, cx, cy))
d4 = cv.GetSubRect(src_arr, (0, cy, cx, cy))
if (src_arr is not dst_arr):
if (not cv.CV_ARE_TYPES_EQ(q1, d1)):
cv.Error(
cv.CV_StsUnmatchedFormats, "cv.ShiftDFT",
"Source and Destination arrays must have the same format",
__FILE__, __LINE__)
cv.Copy(q3, d1)
cv.Copy(q4, d2)
cv.Copy(q1, d3)
cv.Copy(q2, d4)
else:
cv.Copy(q3, tmp)
cv.Copy(q1, q3)
cv.Copy(tmp, q1)
cv.Copy(q4, tmp)
cv.Copy(q2, q4)
cv.Copy(tmp, q2)
def computeSkewAngle(dilation):
dilation_copy = dilation.copy()
contours, hierarchy = cv2.findContours(
dilation_copy, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE) # Detect the contours of all objects
leng = 0
for i in range(len(contours)):
#print len(contours[i])
leng = leng + len(contours[i])
print leng + 1
points = []
pointMat = cv.CreateMat(leng, 1, cv.CV_32SC2)
try:
for i in range(len(contours[i])):
cnt = contours[
i] #### check why it is going list out of index OR EXCEPTION HANDLING ######
#print cnt
for i in range(len(cnt)):
pointMat[i, 0] = tuple(cnt[i][0])
points.append(tuple(cnt[i][0]))
#print pointMat
except:
print 'Exception raised'
print "Unexpected error:", sys.exc_info()[0]
finally:
print 'Hi'
box = cv.MinAreaRect2(points)
box_vtx = [roundxy(p) for p in cv.BoxPoints(box)]
#box_vtx = [cv.Round(pt[0]), cv.Round(pt[1]) for p in cv.BoxPoints(box)]
print box[2]
if box[2] < -45:
skew_angle = box[2] + 90
else:
skew_angle = box[2]
print 'Skew Angle : ', skew_angle
return skew_angle, box_vtx
def track_keypoints(self,grey,prev_grey): try: # we are tracking points between the previous frame and the current frame img0,img1 = prev_grey,grey # reshape the current keypoints into a numpt array required by calcOpticalFlowPyrLK() p0 = np.float32([p for p in self.keypoints]).reshape(-1,1,2) # calculate the optical flow from the previous frame # tp the current frame p1,st,err = cv2.calcOpticalFlowPyLK(img0,img1,p0,None,**self.lk.params) # do the reverse ca;culation : from the current frame to te previous one p0r,st,err = cv2.calcOpticalFlowPyrLK(imag1,imag0,p1,NOne,**self.lk_params) # compute the distance between corresponding points in the two flows d = abs(p0-p0r).reshape(-1,2).max(-1) # if the distance between pairs of points is <1 pixel,set # a value in the 'good' array to True,Otherwise False good = d<1 #Initialize a list to hold new keypoints new_keypoints = list() #cycle through all current and new keypoints and only keep #those that satisfy the 'good' condition above for (x,y),good_flag in zip(p1.reshape(-1,2),good): if not good_flag: continue new_keypoints.append((x,y)) # Draw the keypoint on the image cv2.circle(self.marker_iamge,(x,y),self.feature_size,(0,255,0,0),cv.CV_FILLED,8,0) # set the global keypoint list to the new list self.keypoints = new_keypoints # if we have >6 points,find the best ellipse around them if len(self.keypoints)>6: self.keypoints_matrix = cv.CreateMat(1,len(self.keypoints),cv.CV_32sc2) i = 0 for p in self.keypoints: cv.Set2D(self.keypoints_matrix,0,i,(int(p[0]),int(p[1]))) i = i+1 track_box = cv.FitEllipse2(self.keypoints_matrix) else:
def cal_fromcorners(self, good):
"""
:param good: Good corner positions and boards
:type good: [(corners, ChessboardInfo)]
"""
boards = [b for (_, b) in good]
ipts = self.mk_image_points(good)
opts = self.mk_object_points(boards)
npts = self.mk_point_counts(boards)
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
distortion = cv.CreateMat(8, 1, cv.CV_64FC1) # rational polynomial
else:
distortion = cv.CreateMat(5, 1, cv.CV_64FC1) # plumb bob
cv.SetZero(intrinsics)
cv.SetZero(distortion)
# If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
intrinsics[0, 0] = 1.0
intrinsics[1, 1] = 1.0
cv.CalibrateCamera2(opts,
ipts,
npts,
self.size,
intrinsics,
distortion,
cv.CreateMat(len(good), 3, cv.CV_32FC1),
cv.CreateMat(len(good), 3, cv.CV_32FC1),
flags=self.calib_flags)
self.intrinsics = intrinsics
self.distortion = distortion
# R is identity matrix for monocular calibration
self.R = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.SetIdentity(self.R)
self.P = cv.CreateMat(3, 4, cv.CV_64FC1)
cv.SetZero(self.P)
self.mapx = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.mapy = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1)
self.set_alpha(0.0)
def _get_corners(img, board, refine=True):
"""
Get corners for a particular chessboard for an image
"""
w, h = cv.GetSize(img)
mono = cv.CreateMat(h, w, cv.CV_8UC1)
cv.CvtColor(img, mono, cv.CV_BGR2GRAY)
(ok, corners) = cv.FindChessboardCorners(
mono, (board.n_cols, board.n_rows), cv.CV_CALIB_CB_ADAPTIVE_THRESH
| cv.CV_CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK)
# If any corners are within BORDER pixels of the screen edge, reject the detection by setting ok to false
# NOTE: This may cause problems with very low-resolution cameras, where 8 pixels is a non-negligible fraction
# of the image size. See http://answers.ros.org/question/3155/how-can-i-calibrate-low-resolution-cameras
BORDER = 8
if not all([(BORDER < x < (w - BORDER)) and (BORDER < y < (h - BORDER))
for (x, y) in corners]):
ok = False
if refine and ok:
# Use a radius of half the minimum distance between corners. This should be large enough to snap to the
# correct corner, but not so large as to include a wrong corner in the search window.
min_distance = float("inf")
for row in range(board.n_rows):
for col in range(board.n_cols - 1):
index = row * board.n_rows + col
min_distance = min(min_distance,
_pdist(corners[index], corners[index + 1]))
for row in range(board.n_rows - 1):
for col in range(board.n_cols):
index = row * board.n_rows + col
min_distance = min(
min_distance,
_pdist(corners[index], corners[index + board.n_cols]))
radius = int(math.ceil(min_distance * 0.5))
corners = cv.FindCornerSubPix(
mono, corners, (radius, radius), (-1, -1),
(cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1))
return (ok, corners)
def __init__(self, map_name="hall_inria"):
""" Initialize our interest grid.
@param y_length: int, y_length of the grid [m]
@param x_length: int, Columns of the grid [m]
@param origin: tuple (x,y), where it is going to be located the lower-left corner of the image with respect to the grid.
@param resolution: float, resolution of the grid in [meters/cells]
"""
""" MEMBERS """
""" INIT """
self.__read_map__(map_name) #reads the map and the map metadata
#The image with the detected goal points
self.goal_map=cv.CreateMat(self.map_img.height, self.map_img.width, cv.CV_8UC1)
cv.Set(self.goal_map, 255)
self.goal_list = []
self.hough_rho =1 #- self.map_img.height/600#Biggest == more lines
self.hough_threshold = 10 #Biggest==less lines
print "An instance of InterestGrid has been created"
def detect(image):
image_size = cv.GetSize(image)
# to grayscale
grayscale = cv.CreateImage(image_size, 8, 1)
cv.CvtColor(image, grayscale, cv.CV_RGB2GRAY)
# equalize
cv.EqualizeHist(grayscale, grayscale)
# detections
faces = cv.HaarDetectObjects(grayscale, cascade, cv.CreateMemStorage(),
1.2, 2, cv.CV_HAAR_DO_CANNY_PRUNING, (15, 15))
if faces:
print 'face detected!'
for faceN, ((x, y, w, h), n) in enumerate(faces):
y_offset = y - height_offset
h_expanded = h + expansion
h_expanded_offset = h_expanded + height_offset
if y_offset + h_expanded_offset > FRAME_HEIGHT:
h_expanded_offset = (y_offset +
h_expanded_offset) - FRAME_HEIGHT
if y_offset < 0:
y_offset = 0
windowName = "Face %d" % faceN
sub = cv.GetSubRect(image, (x, y_offset, w, h_expanded_offset))
grow = cv.CreateMat(h_expanded * 4, w * 4, cv.CV_8UC3)
cv.Resize(sub, grow)
print "Showing for ", windowName
cv.ShowImage(windowName, grow)
# cv.ShowImage("Face at (%d, %d)" % (x, y), grow)
cv.Rectangle(image,
(x, y - height_offset), (x + w, y + h + expansion),
cv.RGB(0, 255, 0), 3, 8, 0)
def test_2542670(self):
xys = [(94, 121), (94, 122), (93, 123), (92, 123), (91, 124), (91, 125), (91, 126), (92, 127), (92, 128), (92, 129), (92, 130), (92, 131), (91, 132), (90, 131), (90, 130), (90, 131), (91, 132), (92, 133), (92, 134), (93, 135), (94, 136), (94, 137), (94, 138), (95, 139), (96, 140), (96, 141), (96, 142), (96, 143), (97, 144), (97, 145), (98, 146), (99, 146), (100, 146), (101, 146), (102, 146), (103, 146), (104, 146), (105, 146), (106, 146), (107, 146), (108, 146), (109, 146), (110, 146), (111, 146), (112, 146), (113, 146), (114, 146), (115, 146), (116, 146), (117, 146), (118, 146), (119, 146), (120, 146), (121, 146), (122, 146), (123, 146), (124, 146), (125, 146), (126, 146), (126, 145), (126, 144), (126, 143), (126, 142), (126, 141), (126, 140), (127, 139), (127, 138), (127, 137), (127, 136), (127, 135), (127, 134), (127, 133), (128, 132), (129, 132), (130, 131), (131, 130), (131, 129), (131, 128), (132, 127), (133, 126), (134, 125), (134, 124), (135, 123), (136, 122), (136, 121), (135, 121), (134, 121), (133, 121), (132, 121), (131, 121), (130, 121), (129, 121), (128, 121), (127, 121), (126, 121), (125, 121), (124, 121), (123, 121), (122, 121), (121, 121), (120, 121), (119, 121), (118, 121), (117, 121), (116, 121), (115, 121), (114, 121), (113, 121), (112, 121), (111, 121), (110, 121), (109, 121), (108, 121), (107, 121), (106, 121), (105, 121), (104, 121), (103, 121), (102, 121), (101, 121), (100, 121), (99, 121), (98, 121), (97, 121), (96, 121), (95, 121)]
#xys = xys[:12] + xys[16:]
pts = cv.CreateMat(len(xys), 1, cv.CV_32SC2)
for i,(x,y) in enumerate(xys):
pts[i,0] = (x, y)
storage = cv.CreateMemStorage()
hull = cv.ConvexHull2(pts, storage)
hullp = cv.ConvexHull2(pts, storage, return_points = 1)
defects = cv.ConvexityDefects(pts, hull, storage)
vis = cv.CreateImage((1000,1000), 8, 3)
x0 = min([x for (x,y) in xys]) - 10
x1 = max([x for (x,y) in xys]) + 10
y0 = min([y for (y,y) in xys]) - 10
y1 = max([y for (y,y) in xys]) + 10
def xform(pt):
x,y = pt
return (1000 * (x - x0) / (x1 - x0),
1000 * (y - y0) / (y1 - y0))
for d in defects[:2]:
cv.Zero(vis)
# First draw the defect as a red triangle
cv.FillConvexPoly(vis, [xform(p) for p in d[:3]], cv.RGB(255,0,0))
# Draw the convex hull as a thick green line
for a,b in zip(hullp, hullp[1:]):
cv.Line(vis, xform(a), xform(b), cv.RGB(0,128,0), 3)
# Draw the original contour as a white line
for a,b in zip(xys, xys[1:]):
cv.Line(vis, xform(a), xform(b), (255,255,255))
self.snap(vis)
aperture = 3
k = 0.01
maxStrength = 0.0
threshold = 0.01
nonMaxSize = 3
cv.CornerHarris(im, dst_32f, neighbourhood, aperture, k)
minv, maxv, minl, maxl = cv.MinMaxLoc(dst_32f)
dilated = cv.CloneImage(dst_32f)
cv.Dilate(
dst_32f, dilated
) # By this way we are sure that pixel with local max value will not be changed, and all the others will
localMax = cv.CreateMat(dst_32f.height, dst_32f.width, cv.CV_8U)
cv.Cmp(
dst_32f, dilated, localMax, cv.CV_CMP_EQ
) #compare allow to keep only non modified pixel which are local maximum values which are corners.
threshold = 0.01 * maxv
cv.Threshold(dst_32f, dst_32f, threshold, 255, cv.CV_THRESH_BINARY)
cornerMap = cv.CreateMat(dst_32f.height, dst_32f.width, cv.CV_8U)
cv.Convert(dst_32f, cornerMap) #Convert to make the and
cv.And(cornerMap, localMax, cornerMap) #Delete all modified pixels
radius = 3
thickness = 2
l = []
src = np.array([[225, 0], [545, 44], [0, 398], [450, 480]], np.float32)
dst = np.array([[0, 0], [image_in.width, image_in.height],
[0, image_in.height], [image_in.width, 0]], np.float32)
retval = cv2.getPerspectiveTransform(src, dst)
warp = cv2.warpPerspective(image, retval, (cv.GetSize(image_in)))
output = cv.fromarray(warp)
return output
orig = cv.QueryFrame(capture)
orig2 = cv.QueryFrame(capture2)
processed = cv.CreateImage((orig.width, orig.height), cv.IPL_DEPTH_8U, 1)
grid = cv.CreateImage((orig.width * 2, orig.height), cv.IPL_DEPTH_8U, 3)
storage = cv.CreateMat(orig.width, 1, cv.CV_32FC3)
s = []
draw_grid(grid)
while True:
orig = cv.QueryFrame(capture)
orig2 = cv.QueryFrame(capture2)
#cv.Normalize(orig)
# filter for all yellow and blue - everything else is black
processed = colorFilterCombine(orig, "yellow", "blue", s)
# Some processing and smoothing for easier circle detection
cv.Canny(processed, processed, 5, 70, 3)
cv.Smooth(processed, processed, cv.CV_GAUSSIAN, 7, 7)
#cv.ShowImage('processed2', processed)
def store_proba(self, proba):
# print "Got Image"
if not self.info:
return
# print "Processing"
self.timestamp = proba.header.stamp
I = self.br.imgmsg_to_cv(proba, "8UC1")
self.proba = cv.CloneMat(I)
cv.Threshold(I, self.proba, 0xFE, 0xFE, cv.CV_THRESH_TRUNC)
try:
# (trans,rot) = self.listener.lookupTransform(proba.header.frame_id, '/world', proba.header.stamp)
self.listener.waitForTransform(proba.header.frame_id,
self.target_frame,
proba.header.stamp,
rospy.Duration(1.0))
trans = numpy.mat(
self.listener.asMatrix(self.target_frame, proba.header))
# print "Transformation"
# print trans
dstdir = [trans * v for v in self.dirpts3d]
# print "Destination dir"
# print dstdir
origin = trans * self.origin
origin = origin / origin[3, 0]
# origin = numpy.matrix([0.0, 0.0, origin[2,0] / origin[3,0], 1.0]).T
# print "Origin"
# print origin
self.dstpts2d = cv.CreateMat(4, 2, cv.CV_32F)
for i in range(4):
self.dstpts2d[i, 0] = self.x_floor + (origin[0, 0] - dstdir[i][
0, 0] * origin[2, 0] / dstdir[i][2, 0]) * self.floor_scale
self.dstpts2d[i, 1] = self.y_floor - (origin[1, 0] - dstdir[i][
1, 0] * origin[2, 0] / dstdir[i][2, 0]) * self.floor_scale
# print numpy.asarray(self.dstpts2d)
# print "Source points"
# print numpy.asarray(self.srcpts2d)
# print "Dest points"
# print numpy.asarray(self.dstpts2d)
self.H = cv.CreateMat(3, 3, cv.CV_32F)
cv.FindHomography(self.srcpts2d, self.dstpts2d, self.H)
# print "Homography"
# print numpy.asarray(self.H)
cv.WarpPerspective(cv.GetSubRect(
self.proba, (0, self.horizon_offset, self.proba.width,
self.proba.height - self.horizon_offset)),
self.floor_map,
self.H,
flags=cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS,
fillval=0xFF)
msg = self.br.cv_to_imgmsg(self.floor_map)
msg.header.stamp = proba.header.stamp
msg.header.frame_id = self.target_frame
self.pub.publish(msg)
# print "Publishing image"
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "Exception while looking for transform"
return
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
bg = cv.CreateMat(1024, 1024, cv.CV_8UC3)
cv.Set(bg, cv.RGB(0, 0, 0))
df = DmtxFinder()
cv.NamedWindow("camera", 1)
def mkdmtx(msg):
dm_write = DataMatrix()
dm_write.encode(msg)
pi = dm_write.image # .resize((14, 14))
cv_im = cv.CreateImageHeader(pi.size, cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_im, pi.tostring())
return cv_im
def process_image(self, slider_pos):
global cimg, source_image1, ellipse_size, maxf, maxs, eoc, lastcx, lastcy, lastr
"""
This function finds contours, draws them and their approximation by ellipses.
"""
stor = cv.CreateMemStorage()
# Create the destination images
cimg = cv.CloneImage(self.source_image)
cv.Zero(cimg)
image02 = cv.CloneImage(self.source_image)
cv.Zero(image02)
image04 = cv.CreateImage(cv.GetSize(self.source_image),
cv.IPL_DEPTH_8U, 3)
cv.Zero(image04)
# Threshold the source image. This needful for cv.FindContours().
cv.Threshold(self.source_image, image02, slider_pos, 255,
cv.CV_THRESH_BINARY)
# Find all contours.
cont = cv.FindContours(image02, stor, cv.CV_RETR_LIST,
cv.CV_CHAIN_APPROX_NONE, (0, 0))
maxf = 0
maxs = 0
size1 = 0
for c in contour_iterator(cont):
if len(c) > ellipse_size:
PointArray2D32f = cv.CreateMat(1, len(c), cv.CV_32FC2)
for (i, (x, y)) in enumerate(c):
PointArray2D32f[0, i] = (x, y)
# Draw the current contour in gray
gray = cv.CV_RGB(100, 100, 100)
cv.DrawContours(image04, c, gray, gray, 0, 1, 8, (0, 0))
if iter == 0:
strng = segF + '/' + 'contour1.png'
cv.SaveImage(strng, image04)
color = (255, 255, 255)
(center, size, angle) = cv.FitEllipse2(PointArray2D32f)
# Convert ellipse data from float to integer representation.
center = (cv.Round(center[0]), cv.Round(center[1]))
size = (cv.Round(size[0] * 0.5), cv.Round(size[1] * 0.5))
if iter == 1:
if size[0] > size[1]:
size2 = size[0]
else:
size2 = size[1]
if size2 > size1:
size1 = size2
size3 = size
# Fits ellipse to current contour.
if eoc == 0 and iter == 2:
rand_val = abs((lastr - ((size[0] + size[1]) / 2)))
if rand_val > 20 and float(max(size[0], size[1])) / float(
min(size[0], size[1])) < 1.5:
lastcx = center[0]
lastcy = center[1]
lastr = (size[0] + size[1]) / 2
if rand_val > 20 and float(max(size[0], size[1])) / float(
min(size[0], size[1])) < 1.4:
cv.Ellipse(cimg, center, size, angle, 0, 360, color, 2,
cv.CV_AA, 0)
cv.Ellipse(source_image1, center, size, angle, 0, 360,
color, 2, cv.CV_AA, 0)
elif eoc == 1 and iter == 2:
(int, cntr, rad) = cv.MinEnclosingCircle(PointArray2D32f)
cntr = (cv.Round(cntr[0]), cv.Round(cntr[1]))
rad = (cv.Round(rad))
if maxf == 0 and maxs == 0:
cv.Circle(cimg, cntr, rad, color, 1, cv.CV_AA, shift=0)
cv.Circle(source_image1,
cntr,
rad,
color,
2,
cv.CV_AA,
shift=0)
maxf = rad
elif (maxf > 0 and maxs == 0) and abs(rad - maxf) > 30:
cv.Circle(cimg, cntr, rad, color, 2, cv.CV_AA, shift=0)
cv.Circle(source_image1,
cntr,
rad,
color,
2,
cv.CV_AA,
shift=0)
maxs = len(c)
if iter == 1:
temp3 = 2 * abs(size3[1] - size3[0])
if (temp3 > 40):
eoc = 1
import cv2.cv as cv
capture = cv.CaptureFromCAM(0)
frame1 = cv.QueryFrame(capture)
frame1gray = cv.CreateMat(frame1.height, frame1.width, cv.CV_8U)
cv.CvtColor(frame1, frame1gray, cv.CV_RGB2GRAY)
res = cv.CreateMat(frame1.height, frame1.width, cv.CV_8U)
frame2gray = cv.CreateMat(frame1.height, frame1.width, cv.CV_8U)
w = frame2gray.width
h = frame2gray.height
nb_pixels = frame2gray.width * frame2gray.height
while True:
frame2 = cv.QueryFrame(capture)
cv.CvtColor(frame2, frame2gray, cv.CV_RGB2GRAY)
cv.AbsDiff(frame1gray, frame2gray, res)
cv.ShowImage("After AbsDiff", res)
cv.Smooth(res, res, cv.CV_BLUR, 5, 5)
element = cv.CreateStructuringElementEx(5 * 2 + 1, 5 * 2 + 1, 5, 5,
cv.CV_SHAPE_RECT)
cv.MorphologyEx(res, res, None, None, cv.CV_MOP_OPEN)
cv.MorphologyEx(res, res, None, None, cv.CV_MOP_CLOSE)
cv.Threshold(res, res, 10, 255, cv.CV_THRESH_BINARY_INV)
cv.ShowImage("Image", frame2)
def convertCV32(stacked):
hist64 = cv.fromarray(stacked)
hist32 = cv.CreateMat(hist64.rows, hist64.cols, cv.CV_32FC1)
cv.Convert(hist64, hist32)
return hist32
color_tab = [
cv.CV_RGB(255, 0, 0),
cv.CV_RGB(0, 255, 0),
cv.CV_RGB(100, 100, 255),
cv.CV_RGB(255, 0, 255),
cv.CV_RGB(255, 255, 0)
]
img = cv.CreateImage((500, 500), 8, 3)
rng = cv.RNG(-1)
cv.NamedWindow("clusters", 1)
while True:
cluster_count = randint(2, MAX_CLUSTERS)
sample_count = randint(1, 1000)
points = cv.CreateMat(sample_count, 1, cv.CV_32FC2)
clusters = cv.CreateMat(sample_count, 1, cv.CV_32SC1)
# generate random sample from multigaussian distribution
for k in range(cluster_count):
center = (cv.RandInt(rng) % img.width,
cv.RandInt(rng) % img.height)
first = k * sample_count / cluster_count
last = sample_count
if k != cluster_count:
last = (k + 1) * sample_count / cluster_count
point_chunk = cv.GetRows(points, first, last)
cv.RandArr(rng, point_chunk, cv.CV_RAND_NORMAL,
cv.Scalar(center[0], center[1], 0, 0),
def process_image(self, slider_pos):
"""
This function finds contours, draws them and their approximation by ellipses.
"""
use_this = self.source_image
if self.intensity == False:
cv.Smooth(self.source_image, self.edge, cv.CV_BLUR, 9, 9, 0)
cv.Not(self.source_image, self.edge)
# run the edge dector on gray scale
cv.Canny(self.source_image, self.edge, slider_pos, slider_pos * 3,
3)
# reset
cv.SetZero(self.col_edge)
# copy edge points
cv.Copy(self.source_color, self.col_edge, self.edge)
use_this = self.edge
stor = cv.CreateMemStorage()
# Create the destination images
image02 = cv.CloneImage(use_this)
cv.Zero(image02)
image04 = cv.CreateImage(cv.GetSize(self.source_image),
cv.IPL_DEPTH_8U, 3)
cv.Zero(image04)
# Threshold the source image. This needful for cv.FindContours().
cv.Threshold(use_this, image02, slider_pos, 255, cv.CV_THRESH_BINARY)
# Find all contours.
cont = cv.FindContours(image02, stor, cv.CV_RETR_LIST,
cv.CV_CHAIN_APPROX_NONE, (0, 0))
for c in contour_iterator(cont):
# Number of points must be more than or equal to 6 for cv.FitEllipse2
if len(c) >= 6:
# Copy the contour into an array of (x,y)s
PointArray2D32f = cv.CreateMat(1, len(c), cv.CV_32FC2)
for (i, (x, y)) in enumerate(c):
PointArray2D32f[0, i] = (x, y)
# Draw the current contour in gray
gray = cv.CV_RGB(100, 100, 100)
cv.DrawContours(image04, c, gray, gray, 0, 1, 8, (0, 0))
# Fits ellipse to current contour.
(center, size, angle) = cv.FitEllipse2(PointArray2D32f)
# Convert ellipse data from float to integer representation.
center = (cv.Round(center[0]), cv.Round(center[1]))
size = (cv.Round(size[0] * 0.5), cv.Round(size[1] * 0.5))
# Draw ellipse in random color
color = cv.CV_RGB(random.randrange(256), random.randrange(256),
random.randrange(256))
cv.Ellipse(image04, center, size, angle, 0, 360, color, 2,
cv.CV_AA, 0)
# Show image. HighGUI use.
cv.ShowImage("Result", image04)
import numpy as np
capture = cv.CaptureFromFile('data/airport/input.avi')
nbFrames = int(cv.GetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_COUNT))
fps = cv.GetCaptureProperty(capture, cv.CV_CAP_PROP_FPS)
wait = int(1 / fps * 1000 / 1)
width = int(cv.GetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_WIDTH))
height = int(cv.GetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_HEIGHT))
out_foreground = cv2.VideoWriter('data/airport/testfun2.avi', -1, fps,
(height, width))
gray = cv.CreateImage((width, height), cv.IPL_DEPTH_8U, 1)
background = cv.CreateMat(height, width, cv.CV_32F)
backImage = cv.CreateImage((width, height), cv.IPL_DEPTH_8U, 1)
foreground = cv.CreateImage((width, height), cv.IPL_DEPTH_8U, 1)
output = cv.CreateImage((width, height), 8, 1)
begin = True
threshold = 10
for f in xrange(nbFrames):
frame = cv.QueryFrame(capture)
out_foreground.write(np.uint8(frame))
cv.CvtColor(frame, gray, cv.CV_BGR2GRAY)
if begin:
def Cached(self, name, rows, cols, type):
key = (name, rows, cols)
if not key in self.cache:
self.cache[key] = cv.CreateMat(rows, cols, type)
return self.cache[key]
import cv2.cv as cv
import math
im = cv.LoadImage("../img/build.png", cv.CV_LOAD_IMAGE_GRAYSCALE)
im2 = cv.CloneImage(im)
# Goodfeatureto track algorithm
eigImage = cv.CreateMat(im.height, im.width, cv.IPL_DEPTH_32F)
tempImage = cv.CloneMat(eigImage)
cornerCount = 500
quality = 0.01
minDistance = 10
corners = cv.GoodFeaturesToTrack(im, eigImage, tempImage, cornerCount, quality,
minDistance)
radius = 3
thickness = 2
for (x, y) in corners:
cv.Circle(im, (int(x), int(y)), radius, (255, 255, 255), thickness)
cv.ShowImage("GoodfeaturesToTrack", im)
#SURF algorithm
hessthresh = 1500 # 400 500
dsize = 0 # 1
layers = 1 # 3 10
keypoints, descriptors = cv.ExtractSURF(im2, None, cv.CreateMemStorage(),
(dsize, hessthresh, 3, layers))
def scale_to_recognizer_input_size(image):
mat = cv.CreateMat(RECOG_SIZE, RECOG_SIZE, cv.CV_8UC1)
cv.Resize(image, mat)
return mat
# set kalman transition matrix
kalman.transition_matrix[0, 0] = 1
kalman.transition_matrix[1, 1] = 1
kalman.transition_matrix[2, 2] = 1
kalman.transition_matrix[3, 3] = 1
# set Kalman Filter
cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1))
cv.SetIdentity(kalman.process_noise_cov,
cv.RealScalar(1e-5)) ## 1e-5
cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1))
cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(0.1))
else:
# Kalman prediction with Kalman Correction with the points I have in trajectory_0000.txt
kalman_prediction = cv.KalmanPredict(kalman)
rightPoints = cv.CreateMat(2, 1, cv.CV_32FC1)
rightPoints[0, 0] = x
rightPoints[1, 0] = y
kalman.state_pre[0, 0] = x
kalman.state_pre[1, 0] = y
kalman.state_pre[2, 0] = 0
kalman.state_pre[3, 0] = 0
estimated = cv.KalmanCorrect(kalman, rightPoints)
i = i + 1
print str(x) + " - " + str(y)
# Here we do not have more points to apply the Kalman Correct, so I need to predict the points
for i in range(20):
print E
F = np.load('F.npy')
print "-F:"
print F
R1 = np.array((3, 3))
R2 = np.array((3, 3))
P1 = np.array((3, 4))
P2 = np.array((3, 4))
Q = np.array((4, 4))
cameraMatrix1 = cv.fromarray(cameraMatrix1)
cameraMatrix2 = cv.fromarray(cameraMatrix2)
distCoeffs1 = cv.fromarray(distCoeffs1)
distCoeffs2 = cv.fromarray(distCoeffs2)
R = cv.fromarray(R)
T = cv.fromarray(T)
R1 = cv.CreateMat(3, 3, cv.CV_64FC1)
R2 = cv.CreateMat(3, 3, cv.CV_64FC1)
P1 = cv.CreateMat(3, 4, cv.CV_64FC1)
P2 = cv.CreateMat(3, 4, cv.CV_64FC1)
Q = cv.CreateMat(4, 4, cv.CV_64FC1)
cv.StereoRectify(cameraMatrix1,
cameraMatrix2,
distCoeffs1,
distCoeffs2, (640, 480),
R,
T,
R1,
R2,
P1,
P2,
Q=Q)
def calculate_params(self, file_path):
global lookupTable
print "calculating params for the cameras"
if not os.path.isfile(file_path) and os.access(file_path, os.R_OK):
print "Either file is missing or is not readable"
exit(-1)
stream = open(file_path, "r")
doc = yaml.load(stream)
'''
read from the file the width and the height and asign it to both cameras
read the distorsion model but assign only pumb_bob because that one is requiered by sptam
read the distCoeffs for both cameras and assign the lists to the cameras
read k, make a list of 9 elements with the correct positions out of it and then assign t to both cameras
Read the extrinsics of both cameras, inverse one of them and calculate the R and t from camera_left to camera_left_to_right
make everything be a cv matrix
cv.StereoRectify(K1_mat, K2_mat, distCoeffs1_mat, distCoeffs2_mat, imageSize, R, t_mat, R1, R2, P1, P2,T_disp2depth, flags=cv.CV_CALIB_ZERO_DISPARITY, alpha=-1, newImageSize=(0,0))
make P1 into a list and assign it as camera_left.P
make P2 into a list and assign it as camera_right.P
make R1 into a list and assign it as camera_left.R
make R2 into a list and assign it as camera_right.R
'''
#width and height
self.camera_left.width = doc['camera0']['resolution'][0]
self.camera_left.height = doc['camera0']['resolution'][1]
self.camera_right.width = doc['camera1']['resolution'][0]
self.camera_right.height = doc['camera1']['resolution'][1]
#distortion_model
self.camera_left.distortion_model = "plumb_bob"
self.camera_right.distortion_model = "plumb_bob"
#distortion_coefficients
self.camera_left.D = doc['camera0']['distortion_coefficients']
self.camera_right.D = doc['camera1']['distortion_coefficients']
#K_left
self.camera_left.K[0] = doc['camera0']['intrinsics'][0]
self.camera_left.K[1] = 0.0
self.camera_left.K[2] = doc['camera0']['intrinsics'][2]
self.camera_left.K[3] = 0.0
self.camera_left.K[4] = doc['camera0']['intrinsics'][1]
self.camera_left.K[5] = doc['camera0']['intrinsics'][3]
self.camera_left.K[6] = 0.0
self.camera_left.K[7] = 0.0
self.camera_left.K[8] = 1.0
#K_right
self.camera_right.K[0] = doc['camera1']['intrinsics'][0]
self.camera_right.K[1] = 0.0
self.camera_right.K[2] = doc['camera1']['intrinsics'][2]
self.camera_right.K[3] = 0.0
self.camera_right.K[4] = doc['camera1']['intrinsics'][1]
self.camera_right.K[5] = doc['camera1']['intrinsics'][3]
self.camera_right.K[6] = 0.0
self.camera_right.K[7] = 0.0
self.camera_right.K[8] = 1.0
#R and t from camera left to right
extrinsics_4x4_l = np.array(doc['camera0']['T_BS']['data'])
extrinsics_left = extrinsics_4x4_l.reshape(4, 4)
extrinsics_4x4_r = np.array(doc['camera1']['T_BS']['data'])
extrinsics_right = extrinsics_4x4_r.reshape(4, 4)
inverse = np.linalg.inv(extrinsics_left)
camera_left_to_right = np.matrix(inverse) * np.matrix(extrinsics_right)
camera_left_to_right = camera_left_to_right[0:3, 0:4]
R = camera_left_to_right[0:3, 0:3]
t = camera_left_to_right[0:3, 3]
stream.close()
#Prepare values for rectifying function calling
R1 = cv.CreateMat(3, 3, cv.CV_64F)
R2 = cv.CreateMat(3, 3, cv.CV_64F)
P1 = cv.CreateMat(3, 4, cv.CV_64F)
P2 = cv.CreateMat(3, 4, cv.CV_64F)
K1_arr = np.asarray(self.camera_left.K)
K1_arr = K1_arr.reshape(3, 3)
K2_arr = np.asarray(self.camera_right.K)
K2_arr = K2_arr.reshape(3, 3)
K1_mat_cont = cv.fromarray(
cv2.copyMakeBorder(K1_arr, 0, 0, 0, 0, cv2.BORDER_REPLICATE))
K2_mat_cont = cv.fromarray(
cv2.copyMakeBorder(K2_arr, 0, 0, 0, 0, cv2.BORDER_REPLICATE))
distCoeffs1_array = np.matrix(np.array(self.camera_left.D))
distCoeffs2_array = np.matrix(np.array(self.camera_right.D))
distCoeffs1_mat_cont = cv.fromarray(
cv2.copyMakeBorder(distCoeffs1_array, 0, 0, 0, 0,
cv2.BORDER_REPLICATE))
distCoeffs2_mat_cont = cv.fromarray(
cv2.copyMakeBorder(distCoeffs2_array, 0, 0, 0, 0,
cv2.BORDER_REPLICATE))
R_mat_cont = cv.fromarray(
cv2.copyMakeBorder(R, 0, 0, 0, 0, cv2.BORDER_REPLICATE))
t_mat_cont = cv.fromarray(
cv2.copyMakeBorder(t, 0, 0, 0, 0, cv2.BORDER_REPLICATE))
imageSize = (self.camera_left.width, self.camera_left.height)
'''
t[0,0]=-50.706459062
t[1,0]=0.15556820057
t[2,0]=0.00088938278
'''
T_disp2depth = cv.CreateMat(4, 4, cv.CV_32F)
#######################
cv.StereoRectify(K1_mat_cont,
K2_mat_cont,
distCoeffs1_mat_cont,
distCoeffs2_mat_cont,
imageSize,
R_mat_cont,
t_mat_cont,
R1,
R2,
P1,
P2,
T_disp2depth,
flags=cv.CV_CALIB_ZERO_DISPARITY,
alpha=-1,
newImageSize=(0, 0))
lookupTable.rectifiedFocalLen = P1[0, 0]
lookupTable.rectifiedCenterCol = P1[0, 2]
lookupTable.rectifiedCenterRow = P1[1, 2]
#lookupTable.baseline = cv.norm(t_mat);
cv.InitUndistortRectifyMap(K1_mat_cont, distCoeffs1_mat_cont, R1, P1,
None, None)
cv.InitUndistortRectifyMap(K2_mat_cont, distCoeffs2_mat_cont, R2, P2,
None, None)
#Transform R1,R2 and P1 and P2 and put thhem in the cameras
P1_arr = np.asarray(P1[:, :])
P1_arr = np.squeeze(np.asarray(P1_arr)).flatten()
P2_arr = np.asarray(P2[:, :])
P2_arr = np.squeeze(np.asarray(P2_arr)).flatten()
R1_arr = np.asarray(R1[:, :])
R1_arr = np.squeeze(np.asarray(R1_arr)).flatten()
R2_arr = np.asarray(R2[:, :])
R2_arr = np.squeeze(np.asarray(R2_arr)).flatten()
R_id_arr = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
self.camera_left.R = R_id_arr
self.camera_right.R = np.squeeze(np.asarray(R)).flatten()
self.camera_left.P = P1_arr
self.camera_right.P = P2_arr
# Tx = - fx * B
self.camera_right.P[
3] = -self.camera_right.P[3] * self.camera_right.K[0]
print "finished calculating params, we return"
self.calculated = True
return
def match_template(self, cv_image):
frame = np.array(cv_image, dtype=np.uint8)
W,H = frame.shape[1], frame.shape[0]
w,h = self.template.shape[1], self.template.shape[0]
width = W - w + 1
height = H - h + 1
# Make sure that the template image is smaller than the source
if W < w or H < h:
rospy.loginfo( "Template image must be smaller than video frame." )
return False
if frame.dtype != self.template.dtype:
rospy.loginfo("Template and video frame must have same depth and number of channels.")
return False
# Create copies of the images to modify
frame_copy = frame.copy()
template_copy = self.template.copy()
# Down pyramid the images
for k in range(self.numDownPyrs):
# Start with the source image
W = (W + 1) / 2
H = (H + 1) / 2
frame_small = np.array([H, W], dtype=frame.dtype)
frame_small = cv2.pyrDown(frame_copy)
# frame_window = "PyrDown " + str(k)
# cv.NamedWindow(frame_window, cv.CV_NORMAL)
# cv.ShowImage(frame_window, cv.fromarray(frame_small))
# cv.ResizeWindow(frame_window, 640, 480)
# Prepare for next loop, if any
frame_copy = frame_small.copy()
#Next, do the target
w = (w + 1) / 2
h = (h + 1) / 2
template_small = np.array([h, w], dtype=self.template.dtype)
template_small = cv2.pyrDown(template_copy)
# template_window = "Template PyrDown " + str(k)
# cv.NamedWindow(template_window, cv.CV_NORMAL)
# cv.ShowImage(template_window, cv.fromarray(template_small))
# cv.ResizeWindow(template_window, 640, 480)
# Prepare for next loop, if any
template_copy = template_small.copy()
# Perform the match on the shrunken images
small_frame_width = frame_copy.shape[1]
small_frame_height = frame_copy.shape[0]
small_template_width = template_copy.shape[1]
small_template_height = template_copy.shape[0]
result_width = small_frame_width - small_template_width + 1
result_height = small_frame_height - small_template_height + 1
result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1)
result = np.array(result_mat, dtype = np.float32)
cv2.matchTemplate(frame_copy, template_copy, cv.CV_TM_CCOEFF_NORMED, result)
cv2.imshow("Result", result)
return (0, 0, 100, 100)
# # Find the best match location
# (minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result)
#
# # Transform point back to original image
# target_location = Point()
# target_location.x, target_location.y = maxLoc
#
# return (target_location.x, target_location.y, w, h)
# Find the top match locations
locations = self.MultipleMaxLoc(result, self.numMaxima)
foundPointsList = list()
confidencesList = list()
W,H = frame.shape[1], frame.shape[0]
w,h = self.template.shape[1], self.template.shape[0]
# Search the large images at the returned locations
for currMax in range(self.numMaxima):
# Transform the point to its corresponding point in the larger image
#locations[currMax].x *= int(pow(2.0, self.numDownPyrs))
#locations[currMax].y *= int(pow(2.0, self.numDownPyrs))
locations[currMax].x += w / 2
locations[currMax].y += h / 2
searchPoint = locations[currMax]
print "Search Point", searchPoint
# If we are searching for multiple targets and we have found a target or
# multiple targets, we don't want to search in the same location(s) again
# if self.findMultipleTargets and len(foundPointsList) != 0:
# thisTargetFound = False
# numPoints = len(foundPointsList)
#
# for currPoint in range(numPoints):
# foundPoint = foundPointsList[currPoint]
# if (abs(searchPoint.x - foundPoint.x) <= self.searchExpansion * 2) and (abs(searchPoint.y - foundPoint.y) <= self.searchExpansion * 2):
# thisTargetFound = True
# break
#
# # If the current target has been found, continue onto the next point
# if thisTargetFound:
# continue
# Set the source image's ROI to slightly larger than the target image,
# centred at the current point
searchRoi = RegionOfInterest()
searchRoi.x_offset = searchPoint.x - w / 2 - self.searchExpansion
searchRoi.y_offset = searchPoint.y - h / 2 - self.searchExpansion
searchRoi.width = w + self.searchExpansion * 2
searchRoi.height = h + self.searchExpansion * 2
#print (searchRoi.x_offset, searchRoi.y_offset, searchRoi.width, searchRoi.height)
# Make sure ROI doesn't extend outside of image
if searchRoi.x_offset < 0:
searchRoi.x_offset = 0
if searchRoi.y_offset < 0:
searchRoi.y_offset = 0
if (searchRoi.x_offset + searchRoi.width) > (W - 1):
numPixelsOver = (searchRoi.x_offset + searchRoi.width) - (W - 1)
print "NUM PIXELS OVER", numPixelsOver
searchRoi.width -= numPixelsOver
if (searchRoi.y_offset + searchRoi.height) > (H - 1):
numPixelsOver = (searchRoi.y_offset + searchRoi.height) - (H - 1)
searchRoi.height -= numPixelsOver
mask = (searchRoi.x_offset, searchRoi.y_offset, searchRoi.width, searchRoi.height)
frame_mat = cv.fromarray(frame)
searchImage = cv.CreateMat(searchRoi.height, searchRoi.width, cv.CV_8UC3)
searchImage = cv.GetSubRect(frame_mat, mask)
searchArray = np.array(searchImage, dtype=np.uint8)
# Perform the search on the large images
result_width = searchRoi.width - w + 1
result_height = searchRoi.height - h + 1
result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1)
result = np.array(result_mat, dtype = np.float32)
cv2.matchTemplate(searchArray, self.template, cv.CV_TM_CCOEFF_NORMED, result)
# Find the best match location
(minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result)
maxValue *= 100
# Transform point back to original image
target_location = Point()
target_location.x, target_location.y = maxLoc
target_location.x += searchRoi.x_offset - w / 2 + self.searchExpansion
target_location.y += searchRoi.y_offset - h / 2 + self.searchExpansion
if maxValue >= self.matchPercentage:
# Add the point to the list
foundPointsList.append(maxLoc)
confidencesList.append(maxValue)
# If we are only looking for a single target, we have found it, so we
# can return
if not self.findMultipleTargets:
break
if len(foundPointsList) == 0:
rospy.loginfo("Target was not found to required confidence")
return (target_location.x, target_location.y, w, h)
import cv2.cv as cv
import numpy as np
import time
while True:
for i in range(4000):
a = cv.CreateImage((1024,1024), cv.IPL_DEPTH_8U, 1)
b = cv.CreateMat(1024, 1024, cv.CV_8UC1)
c = cv.CreateMatND([1024,1024], cv.CV_8UC1)
print "pause..."
def match_template(self, frame):
H,W = frame.shape[0], frame.shape[1]
h,w = self.template.shape[0], self.template.shape[1]
# Make sure that the template image is smaller than the source
if W < w or H < h:
rospy.loginfo( "Template image must be smaller than video frame." )
return None
if frame.dtype != self.template.dtype:
rospy.loginfo("Template and video frame must have same depth and number of channels.")
return None
# Create a copy of the frame to modify
frame_copy = frame.copy()
for i in range(self.n_pyr):
frame_copy = cv2.pyrDown(frame_copy)
template_height, template_width = self.template.shape[:2]
# Cycle through all scales starting with the last successful scale
scales = self.scales[self.last_scale:] + self.scales[:self.last_scale - 1]
# Track which scale and rotation gives the best match
maxScore = -1
best_s = 1
best_r = 0
best_x = 0
best_y = 0
for s in self.scales:
for r in self.rotations:
# Scale the template by s
template_copy = cv2.resize(self.template, (int(template_width * s), int(template_height * s)))
# Rotate the template through r degrees
rotation_matrix = cv2.getRotationMatrix2D((template_copy.shape[1]/2, template_copy.shape[0]/2), r, 1.0)
template_copy = cv2.warpAffine(template_copy, rotation_matrix, (template_copy.shape[1], template_copy.shape[0]), borderMode=cv2.BORDER_REPLICATE)
# Use pyrDown() n_pyr times on the scaled and rotated template
for i in range(self.n_pyr):
template_copy = cv2.pyrDown(template_copy)
# Create the results array to be used with matchTempate()
h,w = template_copy.shape[:2]
H,W = frame_copy.shape[:2]
result_width = W - w + 1
result_height = H - h + 1
try:
result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1)
result = np.array(result_mat, dtype = np.float32)
except:
continue
# Run matchTemplate() on the reduced images
cv2.matchTemplate(frame_copy, template_copy, cv.CV_TM_CCOEFF_NORMED, result)
# Find the maximum value on the result map
(minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result)
if maxValue > maxScore:
maxScore = maxValue
best_x, best_y = maxLoc
best_s = s
best_r = r
best_template = template_copy.copy()
self.last_scale = self.scales.index(s)
best_result = result.copy()
# Transform back to original image sizes
best_x *= int(pow(2.0, self.n_pyr))
best_y *= int(pow(2.0, self.n_pyr))
h,w = self.template.shape[:2]
h = int(h * best_s)
w = int(w * best_s)
best_result = cv2.resize(best_result, (int(pow(2.0, self.n_pyr)) * best_result.shape[1], int(pow(2.0, self.n_pyr)) * best_result.shape[0]))
display_result = np.abs(best_result)**3
cv2.imshow("Result", display_result)
best_template = cv2.resize(best_template, (int(pow(2.0, self.n_pyr)) * best_template.shape[1], int(pow(2.0, self.n_pyr)) * best_template.shape[0]))
cv2.imshow("Best Template", best_template)
#match_box = ((best_x + w/2, best_y + h/2), (w, h), -best_r)
return (best_x, best_y, w, h)
(if Kalman filter works correctly,
the yellow segment should be shorter than the red one).
Pressing any key (except ESC) will reset the tracking with a different speed.
Pressing ESC will stop the program.
"""
import urllib2
import cv2.cv as cv
from math import cos, sin, sqrt
import sys
if __name__ == "__main__":
A = [[1, 1], [0, 1]]
img = cv.CreateImage((500, 500), 8, 3)
kalman = cv.CreateKalman(2, 1, 0)
state = cv.CreateMat(2, 1, cv.CV_32FC1) # (phi, delta_phi)
process_noise = cv.CreateMat(2, 1, cv.CV_32FC1)
measurement = cv.CreateMat(1, 1, cv.CV_32FC1)
rng = cv.RNG(-1)
code = -1L
cv.Zero(measurement)
cv.NamedWindow("Kalman", 1)
while True:
cv.RandArr(rng, state, cv.CV_RAND_NORMAL, cv.RealScalar(0),
cv.RealScalar(0.1))
kalman.transition_matrix[0, 0] = 1
kalman.transition_matrix[0, 1] = 1
kalman.transition_matrix[1, 0] = 0