def calculateExtrinsic(img1, img2, cameraIntrinsic):
"""
Takes 2 images, plus intrinsic parameters and returns extrinsic parameters between the two photos
Assumes same camera
"""
sift = cv2.SIFT()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.7*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts2 = np.float32(pts2)
pts1 = np.float32(pts1)
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS)
return F
def epipolar_rectify(imL,imR,show_matches=True):
descriptor_extractor = ORB(n_keypoints=2000)
descriptor_extractor.detect_and_extract(imL)
keypoints1 = descriptor_extractor.keypoints
descriptors1 = descriptor_extractor.descriptors
descriptor_extractor.detect_and_extract(imR)
keypoints2 = descriptor_extractor.keypoints
descriptors2 = descriptor_extractor.descriptors
matches12 = match_descriptors(descriptors1, descriptors2,metric='hamming', cross_check=True)
pts1=keypoints1[matches12[:,0],:]
pts2=keypoints2[matches12[:,1],:]
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_RANSAC)
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]
res,H1,H2=cv2.stereoRectifyUncalibrated(pts1,pts2,F,imL.shape,10)
if show_matches:
fig, ax = plt.subplots(nrows=1, ncols=1)
plot_matches(ax, imL, imR, keypoints1, keypoints2, matches12)
return H1,H2
def findMatchesKnn(image1, image2, filter=True, ratio=True):
'''
Takes images
Returns unsorted matches between keypoints in both images using the kNN match
'''
bf = cv2.BFMatcher()
matches = bf.knnMatch(image1.descs, image2.descs, k=2)
points1 = []
points2 = []
new_matches = []
for m, n in matches:
if m.distance < .75 * n.distance and ratio:
new_matches.append(m)
points1.append(image1.kps[m.queryIdx].pt)
points2.append(image2.kps[m.trainIdx].pt)
elif not ratio:
points1.append(image1.kps[m.queryIdx].pt)
points2.append(image2.kps[m.trainIdx].pt)
new_matches.append(m)
if filter:
F, mask = cv2.findFundamentalMat(np.array(points1), np.array(points2), method=cv2.FM_RANSAC)
new_points1, new_points2, newer_matches = [], [], []
for i in range(len(new_matches)):
if mask[i] == 1:
new_points1.append(image1.kps[new_matches[i].queryIdx].pt)
new_points2.append(image2.kps[new_matches[i].trainIdx].pt)
newer_matches.append(new_matches[i])
return new_points1, new_points2, newer_matches
else:
return points1, points2, new_matches
def stereoMatching(img1, img2):
sift = cv2.SIFT()
k1, d1 = sift.detectAndCompute(img1, None)
k2, d2 = sift.detectAndCompute(img2, None)
# matches
bf = cv2.BFMatcher()
matches = bf.knnMatch(d1, d2, k=2)
good = []
pts1 = []
pts2 = []
for m, n in matches:
if m.distance < 0.75 * n.distance:
good.append(m)
pts2.append(k2[m.trainIdx].pt)
pts1.append(k1[m.queryIdx].pt)
good = sorted(good, key=lambda x: x.distance)
pts1 = np.float32(pts1)
pts2 = np.float32(pts2)
# Testing a hacky outlier detection with findFundamentalMat.
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.RANSAC, 2)
print len(pts1)
# We select only inlier points
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
print len(pts1)
return pts1, pts2
def get_fundamental_matrix(left_image, right_image):
img1 = cv2.imread(left_image, 0)
img2 = cv2.imread(right_image, 0)
kp1, des1 = SIFT.detectAndCompute(img1,None)
kp2, des2 = SIFT.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.8*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.array(pts1, dtype='float32')
pts2 = np.array(pts2, dtype='float32')
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS)
return F
def get_H_list(Apts, Bpts, Nsamples=24):
Npts = len(Apts[0])
if Npts < Nsamples:
Nsamples = Npts
r = random.sample(range(Npts), Nsamples)
E_mat, gdm = cv2.findFundamentalMat(Apts[:2,r].T, Bpts[:2,r].T)#, cv2.FM_8POINT)#, cv2.RANSAC,1,0.99)
return H_from_E( E_mat )
def set_pair_by_match_points(self,pair_a_b,points_a,points_b):
print "!",
f, mask=cv2.findFundamentalMat(points_a, points_b )
positive_count=sum(mask)
ij,f=normalize(pair_a_b,f)
self.F[ij]=f
self.N[ij]=positive_count
def generate_moving_mask_v3(frame_lst,
flows_lst,
var_thres=1,
area_min=100,
area_max=np.inf):
h, w = flows_lst[0].shape[:2]
points1 = np.mgrid[:h, :w].reshape(2, -1).astype(np.float32).T[:, (1, 0)]
bgdModel = np.empty((1, 65), dtype=np.float64)
fgdModel = np.empty((1, 65), dtype=np.float64)
mask = np.empty((h, w), dtype=np.uint8)
Fs = []
masks = []
for frame, flow in zip(frame_lst, flows_lst):
points2 = points1 + flow.reshape(-1, 2)
F, bg = cv2.findFundamentalMat(points1, points2, cv2.FM_RANSAC,
var_thres)
bg = bg.reshape(h, w).astype(np.bool)
fg = ~bg
if np.sum(fg) > 10:
mask[fg] = cv2.GC_PR_FGD
mask[bg] = cv2.GC_PR_BGD
bgdModel[...] = 0
fgdModel[...] = 0
mask, _, _ = cv2.grabCut(frame, mask, None, bgdModel, fgdModel, 1,
cv2.GC_INIT_WITH_MASK)
components = cv2.connectedComponents((mask & 1).astype(np.uint8),
ltype=cv2.CV_16U)
masks.append(filter_components(components, area_min, area_max))
else:
masks.append((0, np.zeros_like(fg, dtype=np.uint16)))
Fs.append(F)
return Fs, masks
def rectify_pair(image_left, image_right, viz=False):
"""Computes the pair's fundamental matrix and rectifying homographies.
Arguments:
image_left, image_right: 3-channel images making up a stereo pair.
Returns:
F: the fundamental matrix relating epipolar geometry between the pair.
H_left, H_right: homographies that warp the left and right image so
their epipolar lines are corresponding rows.
"""
image_a_points, image_b_points = find_feature_points(image_left,
image_right)
f_mat, mask = cv2.findFundamentalMat(image_a_points,
image_b_points,
cv2.RANSAC)
imsize = (image_right.shape[1], image_right.shape[0])
image_a_points = image_a_points[mask.ravel() == 1]
image_b_points = image_b_points[mask.ravel() == 1]
_, H1, H2 = cv2.stereoRectifyUncalibrated(image_a_points,
image_b_points,
f_mat, imsize)
return f_mat, H1, H2
def FfromPoints(cls,
P1,
P2,
method,
ransacThresh,
confidence,
maxiters):
"""
Compute fundamental matrix from two sets of corresponding image points
see https://docs.opencv.org/master/d9/d0c/
group__calib3d.html#gae850fad056e407befb9e2db04dd9e509
"""
# TODO check valid input
# need at least 7 pairs of points
# TODO sort options in a user-friendly manner
fopt = {'7p': cv.FM_7POINT,
'8p': cv.FM_8POINT,
'ransac': cv.FM_RANSAC,
'lmeds': cv.FM_LMEDS}
F, mask = cv.findFundamentalMat(P1, P2,
method=fopt[method],
ransacReprojThreshold=ransacThresh,
confidence=confidence,
maxIters=maxiters)
# print('Fund mat = ', F)
return F
def filterFeatures(p1, p2, K, method):
inliers = 0
total = len(p1)
space = ""
status = []
M = None
if len(p1) < 7:
# not enough points
return None, np.zeros(total), [], []
if method == 'homography':
M, status = cv2.findHomography(p1, p2, cv2.LMEDS, tol)
elif method == 'fundamental':
M, status = cv2.findFundamentalMat(p1, p2, cv2.LMEDS, tol)
elif method == 'essential':
M, status = cv2.findEssentialMat(p1, p2, K, cv2.LMEDS, threshold=tol)
elif method == 'none':
M = None
status = np.ones(total)
newp1 = []
newp2 = []
for i, flag in enumerate(status):
if flag:
newp1.append(p1[i])
newp2.append(p2[i])
p1 = np.float32(newp1)
p2 = np.float32(newp2)
inliers = np.sum(status)
total = len(status)
#print '%s%d / %d inliers/matched' % (space, np.sum(status), len(status))
return M, status, np.float32(newp1), np.float32(newp2)
def returnPose(und1, und2, k):
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(und1, None)
kp2, des2 = sift.detectAndCompute(und2, None)
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)
good = []
pts1 = []
pts2 = []
for i, (m, n) in enumerate(matches):
if m.distance < 1 * n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
E = k.T @ F @ k
retval, R, t, mask = cv2.recoverPose(E, pts1, pts2, k)
return R, t
def findMatches(image1, image2, filter=False):
'''
Takes two images
Returns list of matches between the keypoints in poth images
'''
bf = cv2.BFMatcher(cv2.NORM_L1, crossCheck=True)
matches = bf.match(image1.descs, image2.descs)
points1 = []
points2 = []
for match in matches:
points1.append(image1.kps[match.queryIdx].pt)
points2.append(image2.kps[match.trainIdx].pt)
if filter:
F, mask = cv2.findFundamentalMat(np.array(points1), np.array(points2), method=cv2.FM_RANSAC)
new_points1, new_points2, new_matches = [], [], []
for i in range(len(matches)):
if mask[i] == 1:
new_points1.append(image1.kps[matches[i].queryIdx].pt)
new_points2.append(image2.kps[matches[i].trainIdx].pt)
new_matches.append(matches[i])
return new_points1, new_points2, new_matches
else:
return points1, points2, matches
def solve(images):
on = images[0]
off = images[1]
dist = distance(on, off)
print 'decoding x'
xcoords, width = decode_axis(on, off, images[2::2], 'x')
print 'decoding y'
ycoords, height = decode_axis(on, off, images[3::2], 'y')
cam_points = []
proj_points = []
for i in xrange(on.shape[0]):
print i / on.shape[0]
for j in xrange(on.shape[1]):
x = xcoords[i,j]
y = ycoords[i,j]
if dist[i,j] > 0.25:
cam_points.append((i, j))
proj_points.append((height - y - 1, x))
print len(cam_points), 'points'
print 'finding fundamental matrix'
cam_array = np.array(cam_points) / 1024 - 1
proj_array = np.array(proj_points) / 1024 - 1
F, status = cv2.findFundamentalMat(cam_array, proj_array, cv.CV_FM_RANSAC)
print 'stereo rectify'
cam_array = np.array(cam_points) / 1024 - 1
proj_array = np.array(proj_points) / 1024 - 1
res, H1, H2 = cv2.stereoRectifyUncalibrated(cam_array, proj_array, F, on.shape[:2])
def MatchFunM(ID):
print ID
info = Info.GetVideoInfo(ID)
frame_sift_lst = [x for x in sorted(os.listdir(info['frame_sift_path'])) if x.endswith('.sift')]
pano_sift_lst = [x for x in sorted(os.listdir(info['pano_sift_path'])) if x.endswith('.sift')]
results = np.load('%s/fisher_results.npy'%info['pano_path'])
MM = []
for index, name in enumerate(frame_sift_lst):
Mi = []
frame_short_name = name.split('.')[0]
for i in range(0,results.shape[1]):
pano_name = pano_sift_lst[results[index, i]]
pano_short_name = pano_name.split('.')[0]
kp_pairs = lib_SIFTmatch.flann_match('%s/%s'%(info['frame_sift_path'],frame_short_name),
'%s/%s'%(info['pano_sift_path'],pano_short_name))
#print kp_pairs
try:
(mkp1, mkp2) = zip(*kp_pairs)
mkp1_pts = [ (x[0],x[1]) for x in mkp1 ]
mkp2_pts = [ (x[0],x[1]) for x in mkp2 ]
mkp1_pts = np.float32(mkp1_pts)
mkp2_pts = np.float32(mkp2_pts)
F, mask = cv2.findFundamentalMat(mkp1_pts,mkp2_pts,cv2.FM_RANSAC,20)
q_pts = mkp1_pts[mask.ravel()==1]
t_pts = mkp2_pts[mask.ravel()==1]
Mi.append(len(q_pts))
except:
Mi.append(0)
continue
MM.append(Mi)
np.save('%s/results_fundM'%info['pano_path'],MM)
def sift_get_fmat(img1,
img2,
total=100,
ratio=0.8,
algo=cv2.FM_LMEDS,
random=False,
display=False):
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i, (m, n) in enumerate(matches):
if m.distance < ratio * n.distance:
good.append(m)
sorted_good_mat = sorted(good, key=lambda m: m.distance)
for m in sorted_good_mat:
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.float32(pts1)
pts2 = np.float32(pts2)
print('pts size: ', pts1.size)
assert pts1.size > 2 and pts2.size > 2
F, mask = cv2.findFundamentalMat(pts1, pts2, algo)
if mask is None or np.linalg.matrix_rank(F) != 2:
return None, None, None
# assert np.linalg.matrix_rank(F) == 2
# We select only inlier points
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
if random:
# Randomly sample top-[total] number of points
pts = random.sample(zip(pts1, pts2), min(len(pts1), total))
pts1, pts2 = np.array([ p for p, _ in pts ]), \
np.array([ p for _, p in pts ])
else:
pts1 = pts1[:min(len(pts1), total)]
pts2 = pts2[:min(len(pts1), total)]
if display:
draw_matches(img1, pts1, img2, pts2, good)
return F, pts1, pts2
def feature_matching(self, logging=False, view_match=False):
for i, im_i in enumerate(self.images[:-1]):
for j, im_j in enumerate(self.images[i + 1:i + 2], i + 1):
i_kp, j_kp = [], []
src, dst = [], []
# do knnMatch first and ransac later
matches = self.matcher.knnMatch(im_i.desc, im_j.desc, 2)
for match in matches:
if match[0].distance >= 0.75 * match[1].distance:
continue
kp_i_idx, kp_j_idx = match[0].queryIdx, match[0].trainIdx
src.append(im_i.kp[kp_i_idx].pt)
dst.append(im_j.kp[kp_j_idx].pt)
i_kp.append(kp_i_idx)
j_kp.append(kp_j_idx)
# do fundamentalMat RANSAC
src = np.float32([pt for pt in src])
dst = np.float32([pt for pt in dst])
F, masks = cv2.findFundamentalMat(src,
dst,
cv2.FM_RANSAC,
ransacReprojThreshold=3,
confidence=0.99)
self.F_mats[(i, j)] = F
print("Feature Matching", i, 'vs.', j, '=', np.sum(masks), '/',
src.shape[0])
if logging:
print("Feature Matching", i, 'vs.', j, '=', np.sum(masks),
'/', src.shape[0])
if masks is None:
print("Cannot Find Enough Feature Matching Between " +
str(i) + " and " + str(j))
continue
for k, mask_v in enumerate(masks):
if not mask_v: continue
im_i.set_kp_kp(i_kp[k], j, j_kp[k])
im_j.set_kp_kp(j_kp[k], i, i_kp[k])
if view_match:
win_name = str(i) + " VS. " + str(j)
cv2.namedWindow(win_name)
im1 = copy.deepcopy(im_i.im)
im2 = copy.deepcopy(im_j.im)
canvas = np.column_stack([im1, im2])
offset = im2.shape[1]
for k, mask_v in enumerate(masks):
if not mask_v: continue
kp_i_loc = tuple(np.int32(src[k]))
kp_j_loc = list(np.int32(dst[k]))
kp_j_loc[0] += offset
color = [0] * 3
color[k % 3] = 255
cv2.line(canvas, kp_i_loc, tuple(kp_j_loc),
tuple(color), 1)
#new_size = tuple([int(v / self.downsample) for v in canvas.shape])
new_size = tuple([int(v / 4) for v in canvas.shape])
canvas = cv2.resize(canvas, (new_size[1], new_size[0]))
cv2.imshow(win_name, canvas)
cv2.waitKey(0)
cv2.destroyAllWindows()
def find_homography(img1, img2, kp1, kp2, matches, im_list1, im_list2,
out_dir):
src_pts = np.float32([kp1[m.queryIdx].pt for m in matches[:]])
dst_pts = np.float32([kp2[m.trainIdx].pt for m in matches[:]])
F, F_mask = cv2.findFundamentalMat(src_pts, dst_pts, cv2.FM_RANSAC)
matchesMask = F_mask.ravel().tolist()
draw_params = dict(matchColor=(0, 255, 0),
singlePointColor=None,
matchesMask=matchesMask,
flags=2)
img3 = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, **draw_params)
filename = (im_list1.split('.')[0] + '_' + im_list2.split('.')[0] + '_' +
'Fmatching' + '.' + im_list1.split('.')[1])
cv2.imwrite('./' + out_dir + '/' + filename, img3)
H, H_mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = H_mask.ravel().tolist()
draw_params = dict(matchColor=(0, 255, 0),
singlePointColor=None,
matchesMask=matchesMask,
flags=2)
img4 = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, **draw_params)
filename = (im_list1.split('.')[0] + '_' + im_list2.split('.')[0] + '_' +
'Hmatching' + '.' + im_list1.split('.')[1])
cv2.imwrite('./' + out_dir + '/' + filename, img4)
if np.sum(F_mask) > 0.7 * len(matches):
print('More than 70% percent inliers survive in Fundamental Matrix')
print('We calculate the Homography matrix')
if np.sum(H_mask) > 0.7 * len(matches):
print('Still More than 70% percent inliers survive')
print('Most parts of points can match')
H = H
elif np.sum(H_mask) > 0.2 * len(matches) and np.sum(
H_mask) < 0.7 * len(matches):
print('About 20%-70% percent keypoints survive')
print('Small parts of points can match')
H = H
else:
print('Below 20% percent keypoints survive')
print('Most keypoints cannot match')
H = np.zeros((3, 3))
elif np.sum(F_mask) > 0.2 * len(matches) and np.sum(
F_mask) < 0.7 * len(matches):
print('20%-70% percent inliers survive in Fundamental Matrix')
print('We calculate the Homography matrix')
if np.sum(H_mask) > 0.2 * len(matches) and np.sum(
H_mask) < 0.7 * len(matches):
print('About 20%-70% percent keypoints survive')
print('Small parts of points can match')
H = H
else:
print('Below 20% percent keypoints survive')
print('Most keypoints cannot match')
H = np.zeros((3, 3))
else:
print('Below 20% percent keypoints survive in Fundamental Matrix')
print('Most keypoints cannot match')
H = np.zeros((3, 3))
return H
def epipolarlines(index):
img1 = cv.imread('newimageG' + str(index) + '.jpg', 0)
img2 = cv.imread('newimageD' + str(index) + '.jpg', 0)
sift = cv.xfeatures2d.SIFT_create()
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
flann = cv.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)
good = []
pts1 = []
pts2 = []
for i, (m, n) in enumerate(matches):
if m.distance < 0.8 * n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
F, mask = cv.findFundamentalMat(pts1, pts2, cv.FM_LMEDS)
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
def drawlines(img1, img2, lines, pts1, pts2):
''' img1 - image on which we draw the epilines for the points in img2
lines - corresponding epilines '''
r, c = img1.shape
img1 = cv.cvtColor(img1, cv.COLOR_GRAY2BGR)
img2 = cv.cvtColor(img2, cv.COLOR_GRAY2BGR)
for r, pt1, pt2 in zip(lines, pts1, pts2):
color = tuple(np.random.randint(0, 255, 3).tolist())
x0, y0 = map(int, [0, -r[2] / r[1]])
x1, y1 = map(int, [c, -(r[2] + r[0] * c) / r[1]])
img1 = cv.line(img1, (x0, y0), (x1, y1), color, 1)
img1 = cv.circle(img1, tuple(pt1), 5, color, -1)
img2 = cv.circle(img2, tuple(pt2), 5, color, -1)
return img1, img2
lines1 = cv.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F)
lines1 = lines1.reshape(-1, 3)
img5, img6 = drawlines(img1, img2, lines1, pts1, pts2)
lines2 = cv.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F)
lines2 = lines2.reshape(-1, 3)
img3, img4 = drawlines(img2, img1, lines2, pts2, pts1)
plt.subplot(121), plt.imshow(img5)
plt.subplot(122), plt.imshow(img3)
plt.show()
return
def matches_2():
descriptors_cv2_1 = to_cv2_di(descriptors3)
descriptors_cv2_2 = to_cv2_di(descriptors4)
keypoints_cv2_1 = to_cv2_kplist(detected_keypoints3)
keypoints_cv2_2 = to_cv2_kplist(detected_keypoints4)
bf = cv2.BFMatcher()
img1 = cv2.imread(image_pathes[2])
img2 = cv2.imread(image_pathes[3])
matches = bf.knnMatch(descriptors_cv2_1, descriptors_cv2_2, k=2)
good = []
pts1 = []
pts2 = []
theshold_matching = 0.7
for m, n in matches:
if m.distance < theshold_matching * n.distance:
good.append([m])
pts1.append(keypoints_cv2_1[m.queryIdx].pt)
pts2.append(keypoints_cv2_2[m.trainIdx].pt)
print("matches 2 with 0.8: " + str(len(good)))
img_out = cv2.drawMatchesKnn(img1, keypoints_cv2_1, img2, keypoints_cv2_2,
good, None)
cv2.imwrite("out\\.2_0,8png", img_out)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
#create FundamentalMatrix
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS)
#fx = fy = 721.5
#cx = 690.5
#cy = 172.8
#F[0][0] = fx
#F[0][2] = cx
#F[1][1] = fy
#F[1][2] = cy
# F = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 0]])
#select only -----
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
lines = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F)
lines = lines.reshape(-1, 3)
gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
img1, img2 = drawlines(gray1, gray2, lines, pts1, pts2)
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F)
lines2 = lines2.reshape(-1, 3)
img3, img4 = drawlines(gray2, gray1, lines2, pts2, pts1)
#cv2.imshow('img', img1)
cv2.imwrite("out\\3_08.png", img1)
#cv2.waitKey(8000)
#cv2.imshow('img', img3)
cv2.imwrite("out\\4_08.png", img3)
def fundamental_matrix_estimate(self, pts1, pts2):
F, mask = cv2.findFundamentalMat(
pts1,
pts2,
cv2.FM_RANSAC,
ransacReprojThreshold=self.configs['thresh'],
confidence=self.configs['confidence'])
return mask[:, 0].astype(np.bool), F
def compute_test_fund(self):
im1_pts = self.matches[:,:2]
n = im1_pts.shape[0]
im2_pts = self.matches[:,2:]
F,mask = cv2.findFundamentalMat(np.float32(im1_pts),np.float32(im2_pts),cv2.FM_8POINT)
self.F = F
res = self.compute_residual(im1_pts, im2_pts, n)
return F,res
def step2():
def homography_cv2(src, dst):
src_pts = src.reshape(-1,1,2)
dst_pts = dst.reshape(-1,1,2)
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
return H, mask.ravel().tolist()
def plot_matching(imA,kpsA,imB,kpsB,matches,H,inliers):
imB = imB.copy()
h,w = imA.shape[:2]
transformed_box = cv2.perspectiveTransform(np.float32([[0,0],[0,h-1],[w-1,h-1],[w-1,0]]).reshape(-1,1,2),H)
cv2.polylines(imB,[np.int32(transformed_box)],True,(255,255,255),3, cv2.LINE_AA)
im = cv2.drawMatches(imA,kpsA,imB,kpsB,matches,None,
matchColor = (255,255,0),
singlePointColor = None,
matchesMask = inliers,
flags = 2)
cv2.imshow('feature-matching', im)
def more_points(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONUP:
if x<=imA.shape[1]:
print('points1.append(({}, {}))'.format(x, y))
else:
print('points2.append(({}, {}))'.format(x-imA.shape[1], y))
cv2.setMouseCallback('feature-matching', more_points)
imA = sresize(cv2.imread("Deer/20171128_IMG_7754.JPG"), 0.2)
imB = sresize(cv2.imread("Deer/20171128_IMG_7755.JPG"), 0.2)
# SIFT+FLANN matching
sift = cv2.xfeatures2d.SIFT_create()
(kpsA, desA) = sift.detectAndCompute(imA, None)
(kpsB, desB) = sift.detectAndCompute(imB, None)
flann = cv2.FlannBasedMatcher(dict(algorithm = 0, trees = 5), dict(checks = 50))
# Lowe's ratio test
matches = [m for m,n in flann.knnMatch(desA,desB,k=2) if m.distance < 0.7*n.distance]
if len(matches)>10:
while 1: # Just try again when getting singular matrix
try:
H, inliers = homography_cv2(
np.float32([ kpsA[m.queryIdx].pt for m in matches ]),
np.float32([ kpsB[m.trainIdx].pt for m in matches ]))
break
except np.linalg.linalg.LinAlgError:
continue
else:
print("Not enough matches are found")
sys.exit(1)
print('Homography:')
print(H)
plot_matching(imA,kpsA,imB,kpsB,matches,H,inliers)
print('H inliers:', np.sum(inliers))
inliers = np.flatnonzero(np.array(inliers))
points1 = np.float32([ kpsA[m.queryIdx].pt for m in matches ])[inliers]
points2 = np.float32([ kpsB[m.trainIdx].pt for m in matches ])[inliers]
F, F_inliers = cv2.findFundamentalMat(points1, points2)
F_inliers = np.flatnonzero(F_inliers)
return F, np.dot(np.dot(K.T, F), K), points1[F_inliers].tolist(), points2[F_inliers].tolist()
def fundamental_matrix(points1, points2):
F_b, _ = cv2.findFundamentalMat(points1, points2, cv2.FM_8POINT)
mat = []
mass_cent = [0., 0.]
mass_cent_p = [0., 0.]
for i in range(len(points1)):
mass_cent[0] += points1[i, 0]
mass_cent[1] += points1[i, 1]
mass_cent_p[0] += points2[i, 0]
mass_cent_p[1] += points2[i, 1]
mass_cent = np.divide(mass_cent, float(len(points1)))
mass_cent_p = np.divide(mass_cent_p, float(len(points1)))
scale1 = 0.
scale2 = 0.
for i in range(len(points1)):
scale1 += np.sqrt((points1[i][0] - mass_cent[0])**2 +
(points1[i][1] - mass_cent[1])**2)
scale2 += np.sqrt((points2[i][0] - mass_cent_p[0])**2 +
(points2[i][1] - mass_cent_p[1])**2)
scale1 = scale1 / len(points1)
scale2 = scale2 / len(points1)
scale1 = np.sqrt(2.) / scale1
scale2 = np.sqrt(2.) / scale2
A = np.zeros((8, 9))
for i in range(8):
x1 = (points1[i][0] - mass_cent[0]) * scale1
y1 = (points1[i][1] - mass_cent[1]) * scale1
x2 = (points2[i][0] - mass_cent_p[0]) * scale2
y2 = (points2[i][1] - mass_cent_p[1]) * scale2
row = np.array([x2 * x1, x2 * y1, x2, y2 * x1, y2 * y1, y2, x1, y1, 1])
A[i] = row
U, S, V = np.linalg.svd(A)
F = V[-1]
F = np.reshape(F, (3, 3))
U, S, V = np.linalg.svd(F)
S[2] = 0
F = U @ np.diag(S) @ V
T1 = np.array([
scale1, 0, -scale1 * mass_cent[0], 0, scale1, -scale1 * mass_cent[1],
0, 0, 1
])
T1 = T1.reshape((3, 3))
T2 = np.array([
scale2, 0, -scale2 * mass_cent_p[0], 0, scale2,
-scale2 * mass_cent_p[1], 0, 0, 1
])
T2 = T2.reshape((3, 3))
F = np.transpose(T2) @ F @ T1
F = F / F[-1, -1]
return F, F_b
def GetFundamentalMatrix(img1pts, img2pts, outlierThres=.1, prob=.99):
F, mask = cv2.findFundamentalMat(img1pts,
img2pts,
method=cv2.FM_RANSAC,
param1=outlierThres,
param2=prob)
mask = mask.astype(bool).flatten()
return F, mask
def F_ES(self):
'''
use RANSAC to estimation fundamental matrix
'''
pts1, pts2 = self.match_pts1, self.match_pts2
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)
F = F / F[2, 2]
self.F = F
return F
def get_fundamental_matrix_from_csv(csv1, csv2):
x1 = np.loadtxt(csv1, delimiter=',')
x2 = np.loadtxt(csv2, delimiter=',')
F_im, ransac_mask = cv2.findFundamentalMat(x1,
x2,
method=cv2.FM_RANSAC,
param1=3,
param2=0.995)
return F_im
def main():
files_directory = r'C:\Scratch\IPA_Data\Sampled\15_Ambient_Combined\06_red_green_blue'
image_names = [r'a0_amb.tif', r'b0_amb.tif']
image_paths = []
for filename in image_names:
image_paths.append(os.path.join(files_directory, filename))
img1 = cv.imread(image_paths[0], 1)
img2 = cv.imread(image_paths[1], 1)
surf = cv.xfeatures2d.SURF_create()
surf.setHessianThreshold(400)
# find the keypoints and descriptors with SIFT
kp1, des1 = surf.detectAndCompute(img1, None)
kp2, des2 = surf.detectAndCompute(img2, None)
print(len(kp1))
print(len(kp2))
img3 = cv.drawKeypoints(img1, kp1, None, (255, 0, 0), 4)
img4 = cv.drawKeypoints(img2, kp2, None, (255, 0, 0), 4)
bf = cv.BFMatcher()
matches = bf.knnMatch(des1, des2, k=2)
# Apply ratio test
good = []
pts2 = []
pts1 = []
for i, (m, n) in enumerate(matches):
if m.distance < 0.8 * n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
f_matrix, mask = cv.findFundamentalMat(pts1, pts2, cv.FM_LMEDS)
homog = cv.findHomography(pts1, pts2)
aff = cv.estimateAffine2D(pts1, pts2, np.ones([len(pts1)]), cv.LMEDS, 3,
2000, 0.99, 10)
warp_dst = cv.warpAffine(img1, aff[0], (img2.shape[1], img2.shape[0]))
center = (warp_dst.shape[1] // 2, warp_dst.shape[0] // 2)
cv.imshow('Source image', img2)
cv.imshow('Warp', warp_dst)
cv.waitKey()
def _f_inlier(self, width=256, height=256):
grid = np.indices((height, width)).transpose((1, 2, 0))
grid = np.flip(grid, 2)
grid = grid[self.y_inds, self.x_inds]
flow = self.flow * [width, height]
flow = flow[self.y_inds, self.x_inds]
F, mask = cv2.findFundamentalMat(grid, flow, cv2.FM_RANSAC, 8, 0.9)
num_inlier = np.sum(mask)
return num_inlier / self.tg_rgbvec.shape[0]
def compute_test_fund(self):
im1_pts = self.matches[:, :2]
n = im1_pts.shape[0]
im2_pts = self.matches[:, 2:]
F, mask = cv2.findFundamentalMat(np.float32(im1_pts),
np.float32(im2_pts), cv2.FM_8POINT)
self.F = F
res = self.compute_residual(im1_pts, im2_pts, n)
return F, res
def FindFundamentalRansac(self, kpts1, kpts2):
# Compute Fundamental matrix from a set of corresponding keypoints,
# within a RANSAC scheme
# @param kpts1: list of keypoints of the previous frame
# @param kpts2: list of keypoints of the current frame
kpts1 = np.float32(kpts1)
kpts2 = np.float32(kpts2)
self.F, self.mask = cv2.findFundamentalMat(kpts1, kpts2, cv2.FM_RANSAC)
def get_fundamental_matrix(kpt_locs1, kpt_locs2):
# 3x3
kpt_locs1 = np.asarray(kpt_locs1)
kpt_locs2 = np.asarray(kpt_locs2)
F, mask = cv2.findFundamentalMat(kpt_locs1, kpt_locs2, cv2.FM_RANSAC, 0.1,
0.99)
return F
def computeFundamentalMatrix(self, kps_ref, kps_cur):
F, mask = cv2.findFundamentalMat(kps_ref, kps_cur, cv2.FM_RANSAC, param1=kRansacThresholdPixels, param2=kRansacProb)
if F is None or F.shape == (1, 1):
# no fundamental matrix found
raise Exception('No fundamental matrix found')
elif F.shape[0] > 3:
# more than one matrix found, just pick the first
F = F[0:3, 0:3]
return np.matrix(F), mask
def get_movement_fundamental(self, pts_new, pts_old):
'''get movement between images based on matched points'''
pts_old = np.float32(pts_old) #convert to floats
pts_new = np.float32(pts_new)
F = cv2.findFundamentalMat(pts_old, pts_new, method=cv2.cv.CV_FM_LMEDS) #find fundamental matrix :(
#print F[0]
#get epipolar lines through all of the key points
lines = cv2.computeCorrespondEpilines(pts_old.reshape(-1, 1, 2), 1, F[0])
return lines.reshape(-1, 3) #return the lines
def FundamentalRANSACCompute(kp1,kp2,desc1,desc2,hamming=False,flann=False):
MIN_MATCH_COUNT = 10
if not flann:
if not hamming:
bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)
else:
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(desc1, desc2)
# matches = sorted(matches, key=lambda x: x.distance)
if len(matches) > MIN_MATCH_COUNT:
src_pts = np.float32([kp1[m.queryIdx].pt for m in matches
]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in matches
]).reshape(-1, 1, 2)
M, inlierMask = cv2.findFundamentalMat(src_pts, dst_pts,method=cv2.FM_RANSAC)
return True, M, inlierMask, matches
else:
print('Two few matched points')
return False,None,None,None
else:
FLANN_INDEX_KDITREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDITREE, tree=5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(desc1, desc2, k=2)#k=2 means (m,n) bestMatch vs. betterMatch
good = []
for m, n in matches:
if m.distance < 0.8 * n.distance:
good.append(m)
if len(good) > MIN_MATCH_COUNT:
src_pts = np.float32([kp1[m.queryIdx].pt for m in good
]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good
]).reshape(-1, 1, 2)
M, inlierMask = cv2.findFundamentalMat(src_pts, dst_pts, method=cv2.FM_RANSAC)
return True,M,inlierMask,good
else:
print('Two few matched points')
return False,None,None,None
def main():
img1 = cv2.imread(leftname, 0) #queryimage # left image
img2 = cv2.imread(rightname, 0) #trainimage # right image
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
kp2, des2 = sift.detectAndCompute(img2, None)
# now do best matchings
pts1 = []
pts2 = []
# FLANN parameters
correspondences = find_correspondence(des1, des2, 50)
for lidx, ridx, distance in correspondences:
pts2.append(kp2[ridx].pt)
pts1.append(kp1[lidx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
#print(pts1, pts2)
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS)
#F = run8point(pts1, pts2)
#F = run7point(pts1, pts2)[:3]
# We select only inlier points
#pts1 = pts1[mask.ravel()==1]
#pts2 = pts2[mask.ravel()==1]
def drawlines(img1, img2, lines, pts1, pts2):
''' img1 - image on which we draw the epilines for the points in img2
lines - corresponding epilines '''
r, c = img1.shape
img1 = cv2.cvtColor(img1, cv2.COLOR_GRAY2BGR)
img2 = cv2.cvtColor(img2, cv2.COLOR_GRAY2BGR)
for r, pt1, pt2 in zip(lines, pts1, pts2):
color = tuple(np.random.randint(0, 255, 3).tolist())
x0, y0 = map(int, [0, -r[2] / r[1]])
x1, y1 = map(int, [c, -(r[2] + r[0] * c) / r[1]])
img1 = cv2.line(img1, (x0, y0), (x1, y1), color, 1)
img1 = cv2.circle(img1, tuple(pt1), 5, color, -1)
img2 = cv2.circle(img2, tuple(pt2), 5, color, -1)
return img1, img2
# Find epilines corresponding to points in right image (second image) and
# drawing its lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F)
lines1 = lines1.reshape(-1, 3)
img5, img6 = drawlines(img1, img2, lines1, pts1, pts2)
# Find epilines corresponding to points in left image (first image) and
# drawing its lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F)
lines2 = lines2.reshape(-1, 3)
img3, img4 = drawlines(img2, img1, lines2, pts2, pts1)
plt.subplot(121), plt.imshow(img5)
plt.subplot(122), plt.imshow(img3)
plt.show()
def recover_pose(config, kp1, desc1, kp2, desc2):
flann_params = dict(
algorithm=FLANN_INDEX_LSH,
table_number=6, # 12
key_size=12, # 20
multi_probe_level=1) #2
matcher = cv2.FlannBasedMatcher(
flann_params, {}) # bug : need to pass empty dict (#1329)
raw_matches = matcher.knnMatch(desc1, trainDescriptors=desc2, k=2) #2
logging.info('原始匹配数目： %s', len(raw_matches))
mi, p1, p2, kp_pairs = filter_matches(kp1, kp2, raw_matches)
# p1 = cv2.KeyPoint_convert(kp_pairs[0])
# p1 = cv2.KeyPoint_convert(kp_pairs[1])
if len(p1) < 4:
logging.info('过滤之后匹配数目（ %s ）小于4个，无法定位', len(p1))
return
logging.info('过滤之后匹配数目： %s', len(kp_pairs))
# H, status = cv2.findHomography(p1, p2, cv2.RANSAC, 5.0)
# F, status = cv2.findFundamentalMat(p1, p2)
# H, status = cv2.findHomography(p1, p2, 0)
H, status = None, None
if config.homography:
logging.info("使用 finHomography 过滤匹配结果")
H, status = cv2.findHomography(p1, p2, cv2.RANSAC, 3.0)
elif config.fundamental:
logging.info("使用 findFundamentalMat 过滤匹配结果")
H, status = cv2.findFundamentalMat(p1, p2)
if config.show:
explore_match(config, kp_pairs, status, H)
if status is None:
status = np.ones(len(kp_pairs), np.bool_)
pts1 = np.float64(
[kpp[0].pt for kpp, flag in zip(kp_pairs, status) if flag])
pts2 = np.float64(
[kpp[1].pt for kpp, flag in zip(kp_pairs, status) if flag])
n = len(np.unique(pts2, axis=0))
if n < 9:
logging.info('最终匹配数目（ %s ）小于9个，无法定位', n)
return
K = camera_matrix(config)
E, mask = cv2.findEssentialMat(pts1, pts2, K)
retval, R, t, mask = cv2.recoverPose(E, pts1, pts2, K)
nv = np.matrix(R) * np.float64([0, 0, 1]).reshape(3, 1)
yaw = np.arctan2(nv[0], nv[2])
return yaw, t
def filter_by_homography(self, K, i1, i2, j, filter):
clean = True
tol = float(i1.width) / 200.0 # rejection range in pixels
if tol < 1.0:
tol = 1.0
# print "tol = %.4f" % tol
matches = i1.match_list[j]
if len(matches) < self.min_pairs:
i1.match_list[j] = []
return True
p1 = []
p2 = []
for k, pair in enumerate(matches):
use_raw_uv = False
if use_raw_uv:
p1.append(i1.kp_list[pair[0]].pt)
p2.append(i2.kp_list[pair[1]].pt)
else:
# undistorted uv points should be better, right?
p1.append(i1.uv_list[pair[0]])
p2.append(i2.uv_list[pair[1]])
p1 = np.float32(p1)
p2 = np.float32(p2)
#print "p1 = %s" % str(p1)
#print "p2 = %s" % str(p2)
if filter == "homography":
#method = cv2.RANSAC
method = cv2.LMEDS
M, status = cv2.findHomography(p1, p2, method, tol)
elif filter == "fundamental":
method = cv2.FM_RANSAC # more stable
#method = cv2.FM_LMEDS # keeps dropping more points
M, status = cv2.findFundamentalMat(p1, p2, method, tol)
elif filter == "essential":
# method = cv2.FM_RANSAC
method = cv2.FM_LMEDS
M, status = cv2.findEssentialMat(p1, p2, K, method, threshold=tol)
elif filter == "none":
status = np.ones(len(matches))
else:
# fail
M, status = None, None
print(' %s vs %s: %d / %d inliers/matched' \
% (i1.name, i2.name, np.sum(status), len(status)))
# remove outliers
for k, flag in enumerate(status):
if not flag:
print(" deleting: " + str(matches[k]))
clean = False
matches[k] = (-1, -1)
for pair in reversed(matches):
if pair == (-1, -1):
matches.remove(pair)
return clean
def load(path):
img1 = cv2.imread(path + 'im0.png', 0)
img2 = cv2.imread(path + 'im1.png', 0)
kp1, desc1 = keypoints_and_descriptors(img1)
kp2, desc2 = keypoints_and_descriptors(img2)
flag = cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS
img_plot = cv2.drawKeypoints(img1,
kp1,
None,
color=(0, 255, 0),
flags=flag)
cv2.imwrite(path + '_kp1.png', img_plot)
img_plot = cv2.drawKeypoints(img2,
kp2,
None,
color=(0, 255, 0),
flags=flag)
cv2.imwrite(path + '_kp2.png', img_plot)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(desc1, desc2)
img_plot = cv2.drawMatches(img1,
kp1,
img2,
kp2,
matches,
None,
matchColor=(0, 255, 0),
singlePointColor=(255, 0, 0))
cv2.imwrite(path + '_matches.png', img_plot)
pts1 = to_array(lambda e: e.pt, kp1)
pts2 = to_array(lambda e: e.pt, kp2)
match_arr = to_array(lambda e: (e.queryIdx, e.trainIdx), matches)
pts1 = pts1[match_arr[:, 0], :] - np.array(img1.shape, dtype=np.float) / 2
pts2 = pts2[match_arr[:, 1], :] - np.array(img2.shape, dtype=np.float) / 2
all_ones = np.ones(shape=(match_arr.shape[0], 1))
x = np.hstack((pts1, all_ones, pts2, all_ones))
F, mask = cv2.findFundamentalMat(pts1, pts2, method=cv2.FM_8POINT)
dists = []
for i in range(match_arr.shape[0]):
epi_lines1 = np.dot(F, x[i, :3])
epi_lines1 /= np.linalg.norm(epi_lines1[:2])
dists.append(np.abs(np.sum(x[i, 3:] * epi_lines1)))
plt.figure()
plt.plot(dists)
data = dict(data=x, img1=img1, img2=img2)
return data
def Motion_detect(Image1, Image2, Height, K):
'''
Image1: image 1
Image2: image 2
Height: Height of ego car in metres
K: 3x3 camera intrinsic matrix
'''
orb = cv2.ORB_create()
Keypoints1, Descriptors1 = orb.detectAndCompute(Image1, None)
Keypoints2, Descriptors2 = orb.detectAndCompute(Image2, None)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# Match descriptors.
matches = bf.match(Descriptors1,Descriptors2)
# Sort them in the order of their distance.
# matches = sorted(matches, key = lambda x:x.distance)
kp1_list = np.mat([])
kp2_list = np.mat([])
k = 0
number_of_matches = 3000
for m in matches:
img1Idx = m.queryIdx
img2Idx = m.trainIdx
(img1x, img1y) = Keypoints1[img1Idx].pt
(img2x, img2y) = Keypoints2[img2Idx].pt
if k == 0:
kp1_list = [[img1x,img1y,1]]
kp2_list = [[img2x,img2y,1]]
k = 1
else:
kp1_list = np.append(kp1_list,[[img1x,img1y,1]],axis = 0)
kp2_list = np.append(kp2_list,[[img2x,img2y,1]],axis = 0)
k+=1
if k == number_of_matches:
break
F = cv2.findFundamentalMat(kp1_list[:,0:2], kp2_list[:,0:2], method = cv2.FM_RANSAC)
E = K.T @ F[0] @ K
Image3 = cv2.drawMatches(Image1,kp1_list,Image2,kp2_list,matches[:10]ls
)
plt.imshow(Image3)
plt.show()
Transformation_info = cv2.recoverPose(E, (kp1_list[:,0:2]), (kp2_list[:,0:2]), K)
Rotation = Transformation_info[1]
Translation = Transformation_info[2]
Translation = Translation# / Translation[2]
return Rotation, Translation
def compute_fundamental_matrix(kp1, kp2, method='ransac', reproj_threshold=5.0, confidence=0.99):
"""
Given two arrays of keypoints compute the fundamental matrix
Parameters
----------
kp1 : ndarray
(n, 2) of coordinates from the source image
kp2 : ndarray
(n, 2) of coordinates from the destination image
outlier_algorithm : {'ransac', 'lmeds', 'normal'}
The openCV algorithm to use for outlier detection
reproj_threshold : float
The maximum distances in pixels a reprojected points
can be from the epipolar line to be considered an inlier
confidence : float
[0, 1] that the estimated matrix is correct
Returns
-------
transformation_matrix : ndarray
The 3x3 transformation matrix
mask : ndarray
Boolean array of the outliers
Notes
-----
While the method is user definable, if the number of input points
is < 7, normal outlier detection is automatically used, if 7 > n > 15,
least medians is used, and if 7 > 15, ransac can be used.
"""
if method == 'ransac':
method_ = cv2.FM_RANSAC
elif method == 'lmeds':
method_ = cv2.FM_LMEDS
elif method == 'normal':
method_ = cv2.FM_7POINT
else:
raise ValueError("Unknown outlier detection method. Choices are: 'ransac', 'lmeds', or 'normal'.")
transformation_matrix, mask = cv2.findFundamentalMat(kp1,
kp2,
method_,
reproj_threshold,
confidence)
try:
mask = mask.astype(bool)
except:
pass # pragma: no cover
return transformation_matrix, mask
def compute_F_RANSAC(img1, img2):
x1, x2 = get_pixel_points(img1, img2)
if len(x1) < 8:
raise RuntimeError('Fundamental matrix requires N >= 8 pts')
else:
F, mask = cv2.findFundamentalMat(x1, x2, cv2.FM_RANSAC)
# U,S,V = np.linalg.svd(F)
# S[2] = 0
# F = np.dot(U,np.dot(np.diag(S),V))
return F, mask
def getFundMat(good, kp1, kp2):
'''
good: DMatch object list
kp1,kp2: query key points, train key points
'''
print 'The number of matches used in fundamental matrix is %d'%len(good)
pts1,pts2 = getMatchPts(good, kp1, kp2)
# F, mask = find_fund_mat(pts1,pts2) #custom version of find fundmat
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)
return F, mask
def main(argv):
r1, r2 = [], []
with open('points2') as f:
for s in f:
x1, y1, x2, y2 = map(float, s.split())
r1.append([x1, y1])
r2.append([x2, y2])
p1, p2 = np.array(r1), np.array(r2)
ret, mask = cv2.findFundamentalMat(p1, p2)
print ret
def hartleyRectify(points1, points2, imgSize, M1, M2, D1, D2, F = None):
F, mask = cv2.findFundamentalMat(points1, points2, cv2.FM_RANSAC, 3, 0.99)
#print 'mask\n', mask
retval, H1, H2 = cv2.stereoRectifyUncalibrated(
points1, points2, F, imgSize)
retval, M1i = cv2.invert(M1); retval, M2i = cv2.invert(M2)
R1, R2 = np.dot(np.dot(M1i, H1), M1), np.dot(np.dot(M2i, H2), M2)
map1x, map1y = cv2.initUndistortRectifyMap(M1, D1, R1, M1, imgSize, cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(M2, D2, R2, M2, imgSize, cv2.CV_32FC1)
return (map1x, map1y, map2x, map2y), F
def _find_fundamental_matrix(self, method='RANSAC'):
"""Estimates fundamental matrix
:para method: which way to use for fundamental matrix estimation
RANSAC or DL(deep learning)
"""
if method == 'RANSAC':
self.F, self.Fmask = cv2.findFundamentalMat(
self.match_pts1, self.match_pts2, cv2.FM_RANSAC, 0.1, 0.99)
elif method == 'DL':
print('Need to complete')
def computeFundamentalMatrix(kps_ref, kps_cur):
F, mask = cv2.findFundamentalMat(kps_ref, kps_cur, cv2.FM_RANSAC, 2, 0.99)
if F is None or F.shape == (1, 1):
# no fundamental matrix found
raise Exception('No fundamental matrix found')
elif F.shape[0] > 3:
# more than one matrix found, just pick the first
print('more than one matrix found, just pick the first')
F = F[0:3, 0:3]
return np.matrix(F), mask
def verify_cv2_fundam(kps1, kps2, tentatives, th=1.0, n_iter=10000):
src_pts = np.float32([kps1[m.queryIdx].pt
for m in tentatives]).reshape(-1, 2)
dst_pts = np.float32([kps2[m.trainIdx].pt
for m in tentatives]).reshape(-1, 2)
F, mask = cv2.findFundamentalMat(src_pts, dst_pts, cv2.RANSAC, th, 0.999,
n_iter)
print('cv2 found {} inliers'.format(
int(deepcopy(mask).astype(np.float32).sum())))
return F, mask
def getEpilines(pts1,pts2,width,height):
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS)
pts = np.array([(i,j) for i in range(width) for j in range(height)])
epilines = cv2.computeCorrespondEpilines(pts.reshape(-1,1,2), 2,F)
epilines = epilines.reshape(-1,3)
return epilines
def epipolar_geometry(frame1, frame2):
#sift = cv2.SIFT()
# Find the keypoints and descriptors with SIFT
#kp1, des1 = sift.detectAndCompute(frame1, None)
#kp2, des2 = sift.detectAndCompute(frame2, None)
# Trying ORB instead of SIFT
orb = cv2.ORB()
kp1, des1 = orb.detectAndCompute(frame1, None)
kp2, des2 = orb.detectAndCompute(frame2, None)
des1, des2 = map(numpy.float32, (des1, des2))
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks = 50)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)
good, pts1, pts2 = [], [], []
# Ratio test as per Lowe's paper
for i, (m, n) in enumerate(matches):
if m.distance < 0.8*n.distance:
good.append(m)
pts1.append(kp1[m.queryIdx].pt)
pts2.append(kp2[m.trainIdx].pt)
pts1 = numpy.float32(pts1)
pts2 = numpy.float32(pts2)
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS)
return F, mask
pts1 = pts1[mask.ravel() == 1]
pts2 = pts2[mask.ravel() == 1]
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F)
lines1 = lines1.reshape(-1, 3)
img1, _ = drawlines(frame1, frame2, lines1, pts1, pts2)
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 2, F)
lines2 = lines2.reshape(-1, 3)
img2, _ = drawlines(frame2, frame1, lines2, pts2, pts1)
matplotlib.pyplot.subplot(121)
matplotlib.pyplot.imshow(img1)
matplotlib.pyplot.subplot(122)
matplotlib.pyplot.imshow(img2)
matplotlib.show()
def __FundamentalMatrix(self, point):
# Check if the image is frozen.
# SIGB: The user can frozen the input image presses "f" key.
if self.__isFrozen:
# Insert the new selected point in the queue.
if self.__UpdateQueue(point):
# Get all points selected by the user.
points = np.asarray(self.PointsQueue, dtype=np.float32)
# <000> Get the selected points from the left and right images.
leftPoints = points[1::2]
rightPoints = points[::2]
#print leftPoints
#print rightPoints
# <001> Estimate the Fundamental Matrix.
F, mask = cv2.findFundamentalMat(leftPoints, rightPoints)
# Get each point from left image.
# for point in leftPoints:
for point in leftPoints:
#pass
# <002> Estimate the epipolar line.
rightEpiLine = np.dot(F, point)
# <003> Define the initial and final points of the line.
a, b, c = rightEpiLine
x0 = 640
x1 = 1280
y0 = int(-(c+a*x0)/b)
y1 = int(-(c+a*x1)/b)
# <004> Draws the epipolar line in the input image.
cv2.line(self.__Image, (x0, y0), (x1, y1), (255, 0, 0), 1)
# Get each point from right image.
# for point in rightPoints:
for point in rightPoints:
#pass
# <005> Estimate the epipolar line.
leftEpiLine = np.dot(point.transpose(), F)
a, b, c = leftEpiLine
x0 = 0
x1 = 640
y0 = int(-(c+a*x0)/b)
y1 = int(-(c+a*x1)/b)
cv2.line(self.__Image, (x0, y0), (x1, y1), (0, 0, 255), 1)
def find_essential_matrix(K_matrices, norm_pts1, norm_pts2):
'''Estimate an essential matrix that satisfies the epipolar constraint for all the corresponding points.'''
# K = K1, the calibration matrix of the first camera of the current image pair
K = K_matrices[-2]
# convert to Nx2 arrays for findFundamentalMat
norm_pts1 = np.array([ pt[0] for pt in norm_pts1 ])
norm_pts2 = np.array([ pt[0] for pt in norm_pts2 ])
# inliers (1 in mask) are features that satisfy the epipolar constraint
F, mask = cv2.findFundamentalMat(norm_pts1, norm_pts2, cv2.RANSAC)
E = np.dot(K.T, np.dot(F, K))
return E, mask
def getfundamentals(matches,img1,img2):
pt1 = np.zeros((matches.size/4, 2), 'float32')
pt2 = np.zeros((matches.size/4, 2), 'float32')
i = 0
for m in matches:
pt1[i] = (float(m[0]), float(m[1]))
pt2[i] = (float(m[2]), float(m[3]))
i += 1
F, mask = cv2.findFundamentalMat(pt1, pt2, cv2.FM_LMEDS)
return F, pt1, pt2
def robust_match(p1, p2, matches, config):
'''Computes robust matches by estimating the Fundamental matrix via RANSAC.
'''
if len(matches) < 8:
return np.array([])
p1 = p1[matches[:, 0]][:, :2].copy()
p2 = p2[matches[:, 1]][:, :2].copy()
F, mask = cv2.findFundamentalMat(p1, p2, cv2.cv.CV_FM_RANSAC, config['robust_matching_threshold'], 0.99)
inliers = mask.ravel().nonzero()
return matches[inliers]
def FindFundamentalRansacPro(self, queue):
# Compute Fundamental matrix from a set of corresponding keypoints,
# within a RANSAC scheme
# @param kpts1: list of keypoints of the previous frame
# @param kpts2: list of keypoints of the current frame
temp = queue.get()
kpts1 = temp[0]
kpts2 = temp[1]
F = temp[2]
F, mask = cv2.findFundamentalMat(kpts1, kpts2, cv2.FM_RANSAC)
res = [F, mask]
queue.put(res)
def calibrate_stereo_pair(sensorpair):
images = glob.glob('%s/*.jpg' % root_dir)
img = cv2.imread(images[int(random.random() * len(images))], 0)
height = 485/2
width = 1900 / 5
# The Occam is upright so we transpose here to orient things left-right
# Not sure if necessary
img1 = img[0:height, width*sensorpair:width*(sensorpair + 1)].T
img2 = img[height:485, width*sensorpair:width*(sensorpair + 1)].T
#img1 = cv2.imread('%s/%s/left.jpg',0) #queryimage # left image
#img2 = cv2.imread('%s/%s/right.jpg',0) #trainimage # right image
#cv2.imshow('left image', img1)
#cv2.imshow('right image', img2)
#cv2.waitKey(0)
sift = cv2.SIFT()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.8*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.array(pts1)
pts2 = np.array(pts2)
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS)
output = '%s/sensorpair%s' % (root_dir, sensorpair)
if 'sensorpair%s' % sensorpair not in os.listdir(root_dir):
os.mkdir(output)
np.save(open('%s/fundamental.numpy' % output, 'w'), F)
np.save(open('%s/mask.numpy' % output, 'w'), mask)
def reviewFundamentalErrors(self, fuzz_factor=1.0, interactive=True):
total_removed = 0
# Test fundametal matrix constraint
for i, i1 in enumerate(self.image_list):
# rejection range in pixels
tol = float(i1.width) / 800.0 + fuzz_factor
print "tol = %.4f" % tol
if tol < 0.0:
tol = 0.0
for j, matches in enumerate(i1.match_list):
if i == j:
continue
if len(matches) < self.min_pairs:
i1.match_list[j] = []
continue
i2 = self.image_list[j]
p1 = []
p2 = []
for k, pair in enumerate(matches):
p1.append( i1.kp_list[pair[0]].pt )
p2.append( i2.kp_list[pair[1]].pt )
p1 = np.float32(p1)
p2 = np.float32(p2)
#print "p1 = %s" % str(p1)
#print "p2 = %s" % str(p2)
M, status = cv2.findFundamentalMat(p1, p2, cv2.RANSAC, tol)
size = len(status)
inliers = np.sum(status)
print '%s vs %s: %d / %d inliers/matched' \
% (i1.name, i2.name, inliers, size)
if inliers < size:
total_removed += (size - inliers)
if interactive:
status = self.showMatch(i1, i2, matches, status)
delete_list = []
for k, flag in enumerate(status):
if not flag:
print " deleting: " + str(matches[k])
#match[i] = (-1, -1)
delete_list.append(matches[k])
for pair in delete_list:
self.deletePair(i, j, pair)
return total_removed
def _ransac(self, method, matches, tl, tr):
if len(matches) < 4:
return matches
pl, pr = self._extract_points(matches, tl, tr)
_, inliers = cv2.findFundamentalMat(pl, pr, method,
Detector.RANSAC_D, Detector.RANSAC_C)
good_matches = []
for m, is_inlier in zip(matches, inliers):
if is_inlier:
good_matches.append(m)
return good_matches
def filter_matches_by_fundamental_matrix(key_pts1, key_pts2, matches, threshold):
N = len(matches)
i = 0
I = [x.queryIdx for x in matches]
src_pts = key_pts1[I,:].copy()
I = [x.trainIdx for x in matches]
dst_pts = key_pts2[I,:].copy()
N = len(matches)
[H, mask] = cv2.findFundamentalMat(src_pts, dst_pts, cv2.FM_RANSAC, threshold)
mask = np.nonzero(np.reshape(mask, (-1)))[0]
return [matches[i] for i in mask]
def polynomial_triangulation(u1, P1, u2, P2):
"""
Polynomial (Optimal) triangulation.
Uses Linear-Eigen for final triangulation.
Relative speed: 0.1
(u1, P1) is the reference pair containing normalized image coordinates (x, y) and the corresponding camera matrix.
(u2, P2) is the second pair.
u1 and u2 are matrices: amount of points equals #rows and should be equal for u1 and u2.
The status-vector is based on the assumption that all 3D points have finite coordinates.
"""
P1_full = np.eye(4); P1_full[0:3, :] = P1[0:3, :] # convert to 4x4
P2_full = np.eye(4); P2_full[0:3, :] = P2[0:3, :] # convert to 4x4
P_canon = P2_full.dot(cv2.invert(P1_full)[1]) # find canonical P which satisfies P2 = P_canon * P1
# "F = [t]_cross * R" [HZ 9.2.4]; transpose is needed for numpy
F = np.cross(P_canon[0:3, 3], P_canon[0:3, 0:3], axisb=0).T
# Other way of calculating "F" [HZ (9.2)]
#op1 = (P2[0:3, 3:4] - P2[0:3, 0:3] .dot (cv2.invert(P1[0:3, 0:3])[1]) .dot (P1[0:3, 3:4]))
#op2 = P2[0:3, 0:4] .dot (cv2.invert(P1_full)[1][0:4, 0:3])
#F = np.cross(op1.reshape(-1), op2, axisb=0).T
# Project 2D matches to closest pair of epipolar lines
u1_new, u2_new = cv2.correctMatches(F, u1.reshape(1, len(u1), 2), u2.reshape(1, len(u1), 2))
# For a purely sideways trajectory of 2nd cam, correctMatches() returns NaN for all possible points!
if np.isnan(u1_new).all() or np.isnan(u2_new).all():
F = cv2.findFundamentalMat(u1, u2, cv2.FM_8POINT)[0] # so use a noisy version of the fund mat
u1_new, u2_new = cv2.correctMatches(F, u1.reshape(1, len(u1), 2), u2.reshape(1, len(u1), 2))
# Triangulate using the refined image points
return linear_eigen_triangulation(u1_new[0], P1, u2_new[0], P2) # TODO: replace with linear_LS: better results for points not at Inf
