import cv2
import numpy as np
from matplotlib import pyplot as plt
def drawlines(img1,lines,pts1):
[r,c, _] = img1.shape
#img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
#img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
for r,pt1 in zip(lines,pts1):
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)
return img1
def detectAndMatch(img_src, img_dst):
# feature detection
sift = cv2.xfeatures2d_SIFT.create()
kp1, des1 = sift.detectAndCompute(img_src, None)
kp2, des2 = sift.detectAndCompute(img_dst, None)
# FLANN parameters
flann_idx_kdtree = 0
index_params = dict(algorithm=flann_idx_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 m,n in matches:
if m.distance < 0.8*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
return pts1, pts2
def computeFundamentalMat(kp1, kp2):
kp1 = np.int32(kp1)
kp2 = np.int32(kp2)
[F_mat, mask] = cv2.findFundamentalMat(kp1, kp2, cv2.FM_LMEDS)
kp1_inliers = kp1[mask.ravel()==1]
kp2_inliers = kp2[mask.ravel()==1]
return F_mat, kp1_inliers, kp2_inliers
def computeEpiline(kp, which, F_mat):
lines = cv2.computeCorrespondEpilines(kp.reshape(-1,1,2),which,F_mat)
lines = lines.reshape(-1,3)
return lines
img1 = cv2.imread("right.jpg")
img2 = cv2.imread("left.jpg")
[pts1, pts2] = detectAndMatch(img1, img2)
[F_mat, pts1, pts2] = computeFundamentalMat(pts1, pts2)
lines1 = computeEpiline(pts2, 2, F_mat)
lines2 = computeEpiline(pts1, 1, F_mat)
img3 = drawlines(img1,lines1,pts1)
img4 = drawlines(img2, lines2, pts2)
[ret, H1, H2] = cv2.stereoRectifyUncalibrated(pts1, pts2, F_mat, img1.shape[:-1])
undistor_output1 = cv2.warpPerspective(img1,H1,img1.shape[:-1])
undistor_output2 = cv2.warpPerspective(img2,H2,img2.shape[:-1])
[undistort_map1, rectify_map1] = cv2.initUndistortRectifyMap(H1,None,None,H1,img1.shape[:-1], cv2.CV_16SC2)
# undistor_output1 = cv2.remap(img2, undistort_map1, rectify_map1, cv2.INTER_LINEAR)
plt.subplot(221),plt.imshow(img1)
plt.subplot(222),plt.imshow(undistor_output1)
plt.subplot(223),plt.imshow(img2)
plt.subplot(224),plt.imshow(undistor_output2)
plt.show()