def track_keypoints(self, grey, prev_grey):
# We are tracking points between the previous frame and the
# current frame
img0, img1 = prev_grey, grey
# Reshape the current keypoints into a numpy 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 to the current frame
p1, st, err = cv2.calcOpticalFlowPyrLK(img0, img1, p0, None,
**self.lk_params)
# Do the reverse calculation: from the current frame to the previous frame
try:
p0r, st, err = cv2.calcOpticalFlowPyrLK(img1, img0, 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_image, (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 enough points, find the best fit 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:
# Otherwise, find the best fitting rectangle
track_box = cv2.boundingRect(self.keypoints_matrix)
except:
track_box = None
return track_box
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 cut(disparity, image, threshold):
for i in range(0, image.height):
for j in range(0, image.width):
# keep closer object
if cv.GetReal2D(disparity, i, j) > threshold:
cv.Set2D(disparity, i, j, cv.Get2D(image, i, j))
def _find_corr(matches,
hom=False,
data={},
MAX_PIXEL_DEVIATION=MAX_PIXEL_DEVIATION,
FALLBACK_PIXEL_DEVIATIONS=FALLBACK_PIXEL_DEVIATIONS,
rotation_filter_only=False,
ROT_THRESHOLD_RADIANS=ROT_THRESHOLD_RADIANS):
data['success'] = False # by default
matches = list(matches)
F = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetZero(F)
if not matches or (hom and len(matches) < 4):
return F, []
inliers = cv.CreateMat(1, len(matches), cv.CV_8U)
cv.SetZero(inliers)
pts_q = cv.CreateMat(len(matches), 1, cv.CV_64FC2)
pts_db = cv.CreateMat(len(matches), 1, cv.CV_64FC2)
for i, m in enumerate(matches):
cv.Set2D(pts_q, i, 0, cv.Scalar(*m['query'][:2]))
cv.Set2D(pts_db, i, 0, cv.Scalar(*m['db'][:2]))
# ransac for fundamental matrix. rotation filtering
# TODO multiple RANSAC to get smaller/larger features
if not hom:
if rotation_filter_only:
inliers = [1] * len(matches)
else:
cv.FindFundamentalMat(pts_q,
pts_db,
F,
status=inliers,
param1=MAX_PIXEL_DEVIATION,
param2=CONFIDENCE_LEVEL)
inliers = np.asarray(inliers)[0]
# assumes roll(db) == roll(query)
for i, m in enumerate(matches):
if inliers[i]:
if abs(rot_delta(m, 0)) > ROT_THRESHOLD_RADIANS:
inliers[i] = False
return F, inliers
# homography only. no rotation check
cv.FindHomography(pts_db,
pts_q,
F,
method=cv.CV_RANSAC,
ransacReprojThreshold=MAX_PIXEL_DEVIATION,
status=inliers)
### try rounds of homography calculations ###
i = 0
while i < len(FALLBACK_PIXEL_DEVIATIONS):
if not isHomographyGood(F):
cv.FindHomography(
pts_db,
pts_q,
F,
method=cv.CV_RANSAC,
ransacReprojThreshold=FALLBACK_PIXEL_DEVIATIONS[i],
status=inliers)
i += 1
else:
break
if i >= len(FALLBACK_PIXEL_DEVIATIONS):
cv.FindHomography(pts_db, pts_q, F, method=cv.CV_LMEDS, status=inliers)
if isHomographyGood(F):
data['success'] = True
else:
data['success'] = True
return F, np.asarray(inliers)[0]