def get_cld(im1, im2): #im1 and im2 must be black and white if show_rt: t = time.time() #compute Fundamental matrix F, pts1, pts2 = compute_F(im1, im2) #compute Essential matrix E = compute_E(F, im1, im2) #get translation matrix for translation in the x direction R, T = compute_R_T(E) #get K K = compute_K(im1) #get projection from K, R, T P = compute_P(K, R, T) #quit() Tx = T[2] #need to reshape vector so that stereorectifyuncalibrated can pts1_n = pts1.reshape((pts1.shape[0] * 2, 1)) pts2_n = pts2.reshape((pts2.shape[0] * 2, 1)) #compute rectification transform retval, rec1, rec2 = cv2.stereoRectifyUncalibrated(pts1_n, pts2_n, F, (len(im1), len(im1[0]))) if DEBUGGING: print 'rec1:\n', rec1 print 'rec2:\n', rec2 #apply rectification matrix rec_im1 = cv2.warpPerspective(im1, rec1, (len(im1), len(im1[0]))) rec_im2 = cv2.warpPerspective(im2, rec2, (len(im2), len(im2[0]))) if UNIT_TESTING: cv2.imshow('im1', rec_im1) cv2.waitKey(0) cv2.imshow('im2', rec_im2) cv2.waitKey(0) ''' h = rec_im1.shape[0] w = rec_im1.shape[1] dispim = np.zeros((h,2*w), dtype=np.uint8) dispim[:,0:w] = rec_im1 dispim[:,w:] = rec_im2 for jj in range(0,h,15): cv2.line(dispim, (0,jj), (2*w,jj), (255,255,255), 1) cv2.imshow('Sbs', dispim) cv2.waitKey(0) ''' #get disparity map stereo = cv2.StereoBM(cv2.STEREO_BM_BASIC_PRESET,ndisparities=16, SADWindowSize=15) #rec_im1 = cv2.cvtColor(rec_im1, cv2.COLOR_BGR2GRAY) #rec_im2 = cv2.cvtColor(rec_im2, cv2.COLOR_BGR2GRAY) disparity = stereo.compute(rec_im1, rec_im2) #/ 16 disparityF = disparity.astype(float) maxv = np.max(disparityF.flatten()) minv = np.min(disparityF.flatten()) disparityF = 255.0*(disparityF-minv)/(maxv-minv) disparityU = disparityF.astype(np.uint8) print 'disparity.dtype:', disparity.dtype cv2.imwrite('disparity.jpg', disparityU) if UNIT_TESTING: cv2.imshow('disparity', disparityU) cv2.waitKey(0) plt.subplot(122), plt.imshow(disparityF) plt.show() #get perspective transform (Q) cx = len(im1[0]) / 2 cy = len(im1) / 2 cxp = cx Q = np.asarray([[1, 0, 0, -cx], [0, 1, 0, -cy], [0, 0, 0, f_pix], [0, 0, -1/Tx, (cx-cxp)/Tx]]) #reproject to 3d im_3D = cv2.reprojectImageTo3D(disparityU, Q) if show_rt: print '\nget_cld() runtime:', time.time() - t, 'secs\n' return im_3D
def returnH1_H2(points1, points2, F, size): p1 = points1.reshape( len(points1) * 2, 1) #stackoverflow上需要将(m,2)的点变为(m*2,1),因为不变在c++中会产生内存溢出 p2 = points2.reshape(len(points2) * 2, 1) _, H1, H2 = cv2.stereoRectifyUncalibrated(p1, p2, F, size) #size是宽,高 return H1, H2
def drawEpilines(img1, img2, pts1, pts2, F): # Rectify images ret, h1, h2 = cv2.stereoRectifyUncalibrated(pts1, pts2, F, (img1.shape[1], img1.shape[0])) img1_ = img1.copy() img2_ = img2.copy() # Calculate and draw the epiplines in img1 lines1 = cv2.computeCorrespondEpilines(pts2, 2, F) lines1 = lines1.reshape(-1, 3) imgLeft = drawlines(img1_, lines1, pts1, pts2) # Calculate and draw the epiplines in img2 lines2 = cv2.computeCorrespondEpilines(pts1, 1, F) lines2 = lines2.reshape(-1, 3) imgRight = drawlines(img2_, lines2, pts2, pts1) imgLeftRectified = cv2.warpPerspective(imgLeft, h1, (img1.shape[1], img1.shape[0])) imgRightRectified = cv2.warpPerspective(imgRight, h2, (img2.shape[1], img2.shape[0])) imgLeftRectifiedNoEpi = cv2.warpPerspective(img1, h1, (img1.shape[1], img1.shape[0])) imgRightRectifiedNoEpi = cv2.warpPerspective( img2, h2, (img2.shape[1], img2.shape[0])) return imgLeft, imgRight, imgLeftRectified, imgRightRectified, imgLeftRectifiedNoEpi, imgRightRectifiedNoEpi
def rectify_images(self, img_info): R, t, F = img_info _, H0, H1 = cv2.stereoRectifyUncalibrated(self.sorted_keypts_0, self.sorted_keypts_1, F, self.img0.shape[:2][::-1]) print(10 * "-", "H0", 10 * "-") print(H0) print(24 * "-") print(10 * "-", "H1", 10 * "-") print(H1) print(24 * "-") if self.debug: self.disp_epipoles(self.img0, self.img1, F) # Recalculate F and keypoint locations while accounting for rectification F = (np.linalg.inv(H1)).T @ F @ np.linalg.inv(H0) keypts_0 = np.array([self.sorted_keypts_0], dtype=np.float32) keypts_1 = np.array([self.sorted_keypts_1], dtype=np.float32) self.sorted_keypts_0 = cv2.perspectiveTransform( keypts_0, H0)[0].astype(np.int32, copy=False) self.sorted_keypts_1 = cv2.perspectiveTransform( keypts_1, H1)[0].astype(np.int32, copy=False) # Rectify images img0 = cv2.warpPerspective(self.img0, H0, self.img0.shape[:2][::-1]) img1 = cv2.warpPerspective(self.img1, H1, self.img1.shape[:2][::-1]) # Display epipolar lines if self.debug: self.disp_epipoles(img0, img1, F) return img0, img1, F
def stereo_rectification(img1,img2,sift_params,match_th =0.8): kp1,des1,kp2,des2,_,_ = sift_params index_params = dict(algorithm = 1, trees = 5) search_params = dict(checks=50) flann = cv2.FlannBasedMatcher(index_params,search_params) matches = flann.knnMatch(des1,des2,k=2) # Store best matches pts1 = [] pts2 = [] for i,(m,n) in enumerate(matches): if m.distance < 0.8*n.distance: pts2.append(kp2[m.trainIdx].pt) pts1.append(kp1[m.queryIdx].pt) pts1 = np.float32(pts1) pts2 = np.float32(pts2) F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.RANSAC) # We select only inlier points pts1 = pts1[mask.ravel()==1] pts2 = pts2[mask.ravel()==1] img_size = img1.shape[0:2] p,H1,H2=cv2.stereoRectifyUncalibrated(pts1, pts2, F, img_size) H3= H1.dot(H2) img1_corrected = cv2.warpPerspective(img1, H1, img_size) img2_corrected = cv2.warpPerspective(img2, H3, img_size) return img1_corrected, img2_corrected
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 rectification(left, right, correct_kpts1, correct_kpts2, F): """Returns rectified images using OpenCV rectification algorithm""" """ We have implemented our rectification method based on papar Physically-valid view synthesis by image interpolation, Charles R. Dyer Steven M. Seitz. In this case, we can obtain the matrix of rotation, translation and scale to de-rectify, but this algorithm has a very poor performance compared to OpenCV built-in rectification method. Furthermore, this method is not good enough to calculate the disparity, so we comment the few following lines and we use OpenCV. """ correct_kpts1 = np.array(correct_kpts1) correct_kpts1 = correct_kpts1.reshape((correct_kpts1.shape[0] * 2, 1)) correct_kpts2 = np.array(correct_kpts2) correct_kpts2 = correct_kpts2.reshape((correct_kpts2.shape[0] * 2, 1)) shape = (left.shape[1], left.shape[0]) rectBool, H1, H2 = cv2.stereoRectifyUncalibrated(correct_kpts1, correct_kpts2, F, shape, threshold=1) R1 = cv2.warpPerspective(left, H1, shape) R2 = cv2.warpPerspective(right, H2, shape) return R1, R2
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 rectify_corresp_imgs( img_base: np.ndarray, img_corresp: np.ndarray, key_pts_base: np.array, key_pts_corresp: np.array ) -> tuple: """Rectify correspondent images considering theirs key points Args: - `img_base:np.ndarray`: Base image - `img_corresp:np.ndarray`: Correspondent image - `key_pts_base:np.array`: Base image key points - `key_pts_corresp:np.array`: Corresoondent image key points Return: - Both rectified image """ fund_matrix, mask = cv.findFundamentalMat(key_pts_base, key_pts_corresp, cv.FM_LMEDS) # # We select only inlier points key_pts_base = key_pts_base[mask.ravel() == 1] key_pts_corresp = key_pts_corresp[mask.ravel() == 1] _, h1, h2 = cv.stereoRectifyUncalibrated(key_pts_base, key_pts_corresp, fund_matrix, img_base.shape) ret_img_base = cv.warpPerspective(img_base, h1, (img_base.shape[1], img_base.shape[0])) ret_img_corresp = cv.warpPerspective(img_corresp, h2, (img_corresp.shape[1], img_corresp.shape[0])) return ret_img_base, ret_img_corresp
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 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 RectifyImage(img1, img2, left_points, right_points, F_matrix): # Get extrinsic and intrinsic parameters (ex, h1, h2) = cv2.stereoRectifyUncalibrated(left_points, right_points, F_matrix, img1.shape[:2]) # Get combination transform and warp the image comb_trans = np.linalg.inv(h1).dot(h2) im_warp = cv2.warpPerspective(img1, comb_trans, (img2.shape[1], img1.shape[0])) return im_warp
def EpipolarGeometry(pts1, pts2, F, maskF, FT, maskE): ############ to adapt ########################## img1 = cv.pyrDown(cv.imread('Images/leftT2.jpg', 0)) img2 = cv.pyrDown(cv.imread('Images/rightT2.jpg', 0)) ################################################# r, c = img1.shape # Trouver les points d'interet en utilisant la matrice fondamentale pts1F = pts1[maskF.ravel() == 1] pts2F = pts2[maskF.ravel() == 1] # trouver les droite épipolaire dans l'image de droite en utilisant la matrice fondamentale lines1 = cv.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) img5, img6 = drawlines(img1, img2, lines1, pts1F, pts2F) # trouver les droite épipolaire dans l'image de gauche en utilisant la matrice fondamentale lines2 = cv.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(img2, img1, lines2, pts2, pts1) plt.figure('Fright') plt.subplot(121), plt.imshow(img5) plt.subplot(122), plt.imshow(img6) plt.figure('Fleft') plt.subplot(121), plt.imshow(img4) plt.subplot(122), plt.imshow(img3) # Trouver les points d'interet en utilisant la matrice essentiel pts1 = pts1[maskE.ravel() == 1] pts2 = pts2[maskE.ravel() == 1] # trouver les droite épipolaire dans l'image de droite en utilisant la matrice essentiel lines1 = cv.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, FT) lines1 = lines1.reshape(-1, 3) img5T, img6T = drawlines(img1, img2, lines1, pts1, pts2) plt.figure('FTright') plt.subplot(121), plt.imshow(img5T) plt.subplot(122), plt.imshow(img6T) # trouver les droite épipolaire dans l'image de gauche en utilisant la matrice essentiel lines2 = cv.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, FT) lines2 = lines2.reshape(-1, 3) img3T, img4T = drawlines(img2, img1, lines2, pts2, pts1) plt.figure('FTleft') plt.subplot(121), plt.imshow(img4T) plt.subplot(122), plt.imshow(img3T) plt.show() # calculer les homographies qui permettent pour que les droites epipolaire soit # dans des lignes correspondantes retval, H1, H2 = cv.stereoRectifyUncalibrated(pts1, pts2, F, (c, r)) print('H1\n', H1) print('H2\n', H2) # Faire une transformation de perspective à l'aide des matrice de homographie im_dst1 = cv.warpPerspective(img1, H1, (c, r)) im_dst2 = cv.warpPerspective(img2, H2, (c, r)) cv.namedWindow('left', 0) cv.imshow('left', im_dst1) cv.namedWindow('right', 0) cv.imshow('right', im_dst2) cv.waitKey(1)
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 do_rectification(intrinsic_matrix, distortion_coeffs, refined_mtx, rectify_F, size, left_img, left_idx, left_pts, right_img, right_idx, right_pts, prefix="remap", p=dflt_params): img_w, img_h = size # No threshold because we selected only inliers, above cal_success, H1, H2 = cv.stereoRectifyUncalibrated(left_pts, right_pts, rectify_F, (img_w, img_h), threshold=3.0) if cal_success is not True: raise ValueError("Failed to rectify images.") print_message(f"Calculated Homography Matrices:\nH1: {H1}\nH2: {H2}", params=p) # See last comment in https://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html#initundistortrectifymap # Also slide 25: http://ece631web.groups.et.byu.net/Lectures/ECEn631%2014%20-%20Calibration%20and%20Rectification.pdf R1 = np.linalg.inv(intrinsic_matrix).dot(H1).dot(intrinsic_matrix) R2 = np.linalg.inv(intrinsic_matrix).dot(H2).dot(intrinsic_matrix) print_message(f"R Matrices for undistort mappings\nR1: {R1}\nR2: {R2}") map1_1, map1_2 = cv.initUndistortRectifyMap(intrinsic_matrix, distortion_coeffs, R1, refined_mtx, (img_w, img_h), cv.CV_32FC1) map2_1, map2_2 = cv.initUndistortRectifyMap(intrinsic_matrix, distortion_coeffs, R2, refined_mtx, (img_w, img_h), cv.CV_32FC1) print_message(f"Rectify maps:\nmap1_1: {map1_1}\nmat1_2: {map1_2} ", params=p) print_message(f"Rectify maps:\nmap2_1: {map2_1}\nmat2_2: {map2_2} ", params=p) rectified_images = [] left_remap = cv.remap(left_img, map1_1, map1_2, cv.INTER_LANCZOS4) rectified_images.append( (left_remap, f"{prefix}_{left_idx}-{right_idx}.bmp")) right_remap = cv.remap(right_img, map2_1, map2_2, cv.INTER_LANCZOS4) rectified_images.append( (right_remap, f"{prefix}_{right_idx}-{left_idx}.bmp")) output = [cv_save(f, i, params=p) for i, f in rectified_images] return output
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. """ sift = cv2.SIFT() height, width, depth = image_left.shape # find features and descriptors kp1, des1 = sift.detectAndCompute(image_left, None) kp2, des2 = sift.detectAndCompute(image_right, 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) pts1 = [] pts2 = [] # ratio scientifically chosen to be best for i, (m, n) in enumerate(matches): if m.distance < 0.65*n.distance: pts2.append(kp2[m.trainIdx].pt) pts1.append(kp1[m.queryIdx].pt) pts1 = np.float32(pts1) pts2 = np.float32(pts2) fMat, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC, 3, 0.99) # cv2.FM_LMEDS) # this appears to work the same h1 = np.empty((3, 3)) h2 = np.empty((3, 3)) cv2.stereoRectifyUncalibrated( pts1.flatten(), pts2.flatten(), fMat, (height, width), h1, h2, threshold=3) return fMat, h1, h2
def hartleyRectify(points1, points2, imgSize, M1, M2, D1, D2, F): # 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 rectify_stereo_pair_uncalibrated(imgL, imgR, threshold): width, height = imgL.shape[:2] F, mask, left_points, right_points = findFundementalMatrix(imgL, imgR, threshold) # linesL, linesR = calcualteEpilines(left_points, right_points, F) # img5, img6 = drawlines(imgL.copy(), imgR.copy(), linesL, left_points, right_points) # img3, img4 = drawlines(imgR.copy(), imgL.copy(), linesR, right_points, left_points) # pyplot.subplot(121), pyplot.imshow(img5) # pyplot.subplot(122), pyplot.imshow(img3) # pyplot.show() # Rectify the images ret, h_left, h_right = cv2.stereoRectifyUncalibrated(left_points, right_points, F, (imgL.shape[1], imgL.shape[0])) # S = rectify_shearing(h_left, h_right, (imgL.shape[1], imgL.shape[0])) # h_left = S.dot(h_left) # Apply the rectification transforms to the images # camera_matrix = calibrator.camera_matrix # distortion = calibrator.distortion_coeff # imgsize = (imgL.shape[1], imgL.shape[0]) # map1x, map1y, map2x, map2y = remap(camera_matrix, distortion, h_left, h_right, imgsize) # # rectified_left = cv2.remap(imgL, map1x, map1y, # interpolation=cv2.INTER_LINEAR) # # rectified_right = cv2.remap(imgR, map2x, map2y, # interpolation=cv2.INTER_LINEAR) rectified_left = cv2.warpPerspective(imgL, h_left, (height, width), flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) rectified_right = cv2.warpPerspective(imgR, h_right, (height, width), flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) # ## DRAW RECALCULATED EPILINES ## F, mask, left_points, right_points = findFundementalMatrix(rectified_left, rectified_right, 0.8) # # linesL, linesR = calcualteEpilines(left_points, right_points, F) # rectified_left, img6 = drawlines(rectified_left.copy(), rectified_right.copy(), linesL, left_points, right_points) # rectified_right, img4 = drawlines(rectified_right.copy(), rectified_left.copy(), linesR, right_points, left_points) # pyplot.subplot(121), pyplot.imshow(rectified_left) # pyplot.subplot(122), pyplot.imshow(rectified_right) # pyplot.show() ## Display rectified images ## cv2.imshow('Left RECTIFIED', rectified_left) cv2.imshow('Right RECTIFIED', rectified_right) pyplot.show() cv2.waitKey(0) return rectified_left, rectified_right
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. """ img1 = image_left # queryimage # left image img2 = image_right # trainimage # right image 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 m, n in matches: if m.distance / n.distance < .75: good.append(m) pts1 = np.float32([kp1[m.queryIdx].pt for m in good]) pts2 = np.float32([kp2[m.trainIdx].pt for m in good]) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC) pts1 = pts1.reshape(-1, 1, 2) pts2 = pts2.reshape(-1, 1, 2) width, height = img1.shape[:2] r, H_left, H_right = cv2.stereoRectifyUncalibrated(pts1, pts2, F, (width, height)) return F, H_left, H_right
def stereo_calibrate_two_homography_uncalib(self): """Calibrate camera and construct Homography.""" # init camera calibrations rt, self.M1, self.d1, self.r1, self.t1 = cv2.calibrateCamera( self.objpoints, self.imgpoints_l, self.img_shape, None, None) rt, self.M2, self.d2, self.r2, self.t2 = cv2.calibrateCamera( self.objpoints, self.imgpoints_r, self.img_shape, None, None) # config flags = 0 #flags |= cv2.CALIB_FIX_ASPECT_RATIO flags |= cv2.CALIB_USE_INTRINSIC_GUESS #flags |= cv2.CALIB_SAME_FOCAL_LENGTH #flags |= cv2.CALIB_ZERO_TANGENT_DIST flags |= cv2.CALIB_RATIONAL_MODEL #flags |= cv2.CALIB_FIX_K1 #flags |= cv2.CALIB_FIX_K2 #flags |= cv2.CALIB_FIX_K3 #flags |= cv2.CALIB_FIX_K4 #flags |= cv2.CALIB_FIX_K5 #flags |= cv2.CALIB_FIX_K6 stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 100, 1e-5) # stereo calibration procedure ret, self.M1, self.d1, self.M2, self.d2, R, T, E, F = cv2.stereoCalibrate( self.objpoints, self.imgpoints_l, self.imgpoints_r, self.M1, self.d1, self.M2, self.d2, self.img_shape, criteria=stereocalib_criteria, flags=flags) assert ret < 1.0, "[ERROR] Calibration RMS error < 1.0 (%i). Re-try image capture." % ( ret) print("[OK] Calibration successful w/ RMS error=" + str(ret)) F_test, mask = cv2.findFundamentalMat( np.array(self.imgpoints_l).reshape(-1, 2), np.array(self.imgpoints_r).reshape(-1, 2), cv2.FM_RANSAC, 2) # try ransac and other methods too. res, self.H1, self.H2 = cv2.stereoRectifyUncalibrated( np.array(self.imgpoints_l).reshape(-1, 2), np.array(self.imgpoints_r).reshape(-1, 2), F_test, self.img_shape, 2)
def calculateDisparity(pointsImage_L, pointsImage_R, focal_l, imageLeft, imageRight): """ Retifica as imagens imageLeft e imageRight utilizandos o método stereoRectifyUncalibrated da openCV :param pointsImage_L: Pontos em coordenadas em pixels da imagem da esquerda :param pointsImage_R: Pontos em coordenadas em pixels da imagem da direita :param focal_l: Distância focal da câmera da esquerda :param imageLeft: Caminho da imagem da esquerda :param imageRight: Caminho da imagem da esquerda :return disparity, depth, depthplot: Mapa de disparidade, mapa de profundidade, mapa de profundidade normalizado """ imSL = cv2.resize(imageLeft, (640, 480)) imSR = cv2.resize(imageRight, (640, 480)) fundamental_matrix, inliers = cv2.findFundamentalMat(pointsImage_SLf, pointsImage_SRf, method=cv2.FM_LMEDS) retval, H1, H2 = cv2.stereoRectifyUncalibrated( pointsImage_SLf, pointsImage_SRf, fundamental_matrix, imgSize=(640, 480), threshold=0, ) imgL_undistorted = cv2.warpPerspective(imSL, H1, (640, 480)) #plt.imshow(imgL_undistorted,'gray') #plt.show() imgR_undistorted = cv2.warpPerspective(imSR, H2, (640, 480)) #plt.imshow(imgR_undistorted,'gray') #plt.show() # perspective transformation matrix # https://medium.com/@omar.ps16/stereo-3d-reconstruction-with-opencv-using-an-iphone-camera-part-iii-95460d3eddf0 Q = np.float64([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, focal_l * 0.05, 0], [0, 0, 0, 1]]) stereo = cv2.StereoBM_create(numDisparities=48, blockSize=5) disparity = stereo.compute(imgL_undistorted, imgR_undistorted) depth = cv2.reprojectImageTo3D(disparity, Q) depthplot = cv2.normalize(depth, depth, 0, 255, cv2.NORM_MINMAX, cv2.CV_8UC1) return disparity, depth, depthplot
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. """ # Extract features sift = cv2.SIFT() kp_left, desc_left = sift.detectAndCompute(image_left, None) kp_right, desc_right = cv2.SIFT().detectAndCompute(image_right, None) index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=TREES) search_params = dict(checks=CHECKS) flann = cv2.FlannBasedMatcher(index_params, search_params) matches = flann.knnMatch(desc_left, desc_right, k=KNN_ITERS) # Store all the good matches as per Lowe's ratio test good = [] for m, n in matches: if m.distance < LOWE_RATIO * n.distance: good.append(m) # Pick out the left and right points from the good matches pts_left = np.float32([kp_left[m.queryIdx].pt for m in good]).reshape(-1, 1, 2) pts_right = np.float32([kp_right[m.trainIdx].pt for m in good]).reshape(-1, 1, 2) # Compute the fundamental matrix F, mask = cv2.findFundamentalMat(pts_left, pts_right, cv2.FM_RANSAC) pts_left = pts_left[mask.ravel() == 1] pts_right = pts_right[mask.ravel() == 1] # Rectify the images width, height, _ = image_left.shape _, h1, h2 = cv2.stereoRectifyUncalibrated(pts_left, pts_right, F, (width, height)) # Return the fundamental matrix, # the homography for warping the left image, # and the homography for warping the right image return F, h1, h2
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. """ # Initiate SIFT detector sift = cv2.SIFT() # find the keypoints and descriptors with SIFT kp1, des1 = sift.detectAndCompute(image_left, None) kp2, des2 = sift.detectAndCompute(image_right, 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) # store all the good matches as per Lowe's ratio test good = [] for m, n in matches: if m.distance < 0.68 * n.distance: good.append(m) src_pts = np.float32([kp1[m.queryIdx].pt for m in good]) dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]) # find the fundamental matrix F, mask = cv2.findFundamentalMat(src_pts, dst_pts, cv2.RANSAC, 3, 0.99) src_pts = src_pts.flatten() dst_pts = dst_pts.flatten() # rectify the images, produce the homographies: H_left and H_right retval, H_left, H_right = cv2.stereoRectifyUncalibrated( src_pts, dst_pts, F, image_left.shape[:2]) return F, H_left, H_right
def get_rectified_stereo(im_left, im_right): imsize = im_left.shape[1], im_right.shape[0] pts1, pts2, _ = get_sift_matches(im_left, im_right) F, pts1_inliers, pts2_inliers = get_fundamental_mat(pts1, pts2) retval, H1, H2 = cv2.stereoRectifyUncalibrated(pts1_inliers, pts2_inliers, F, imsize) # retval, H1, H2 = cv2.stereoRectifyUncalibrated(pts1.astype(int32), # pts2.astype(int32), # F, imsize) assert retval, 'failed to estimate homographies for stereo rectification1' im_left_rect = cv2.warpPerspective(im_left, H1, imsize) im_right_rect = cv2.warpPerspective(im_right, H2, imsize) return im_left_rect, im_right_rect
def rectify_images(img1, img2): sift = cv2.xfeatures2d.SIFT_create() # find the keypoints and descriptors with SIFT kp1, des1 = sift.detectAndCompute(img1, None) kp2, des2 = sift.detectAndCompute(img2, 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) # store all the good matches as per Lowe's ratio test. good = [] pts1 = [] pts2 = [] for m, n in matches: if m.distance < 0.7 * 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) pts1 = pts1[:, :][mask.ravel() == 1] pts2 = pts2[:, :][mask.ravel() == 1] pts1 = np.int32(pts1) pts2 = np.int32(pts2) p1fNew = pts1.reshape((pts1.shape[0] * 2, 1)) p2fNew = pts2.reshape((pts2.shape[0] * 2, 1)) retBool, rectmat1, rectmat2 = cv2.stereoRectifyUncalibrated( p1fNew, p2fNew, F, img1.shape[:2]) dst11 = cv2.warpPerspective(img1, rectmat1, img1.shape[:2]) dst22 = cv2.warpPerspective(img2, rectmat2, img2.shape[:2]) #plt.imshow(dst22) return dst11, dst22
def rectify_images_sift(image_A, image_B, window_size=16, stride=8, method="greedy", name="p1"): """Rectify two stereo images.""" print("Finding matching points") match_A, match_B = compute_match_sift(image_A, image_B, method=method) print("Finding Fundamantel Matrix") F, mask = cv2.findFundamentalMat(match_A, match_B) print("Computing homography") ret, H1, H2 = cv2.stereoRectifyUncalibrated(match_A, match_B, F, image_A.shape[0:2]) print("Rectifying images") new_img_A = cv2.warpPerspective(image_A, H1, image_A.shape[0:2]) new_img_B = cv2.warpPerspective(image_B, H2, image_A.shape[0:2]) cv2.imwrite("output/rect_sift_" + method + "_" + name + "_a" + ".png", new_img_A) cv2.imwrite("output/rect_sift_" + method + "_" + name + "_b" + ".png", new_img_B) return new_img_A, new_img_B
def rectifyAndPlot(img1_path, img2_path, F, pts1, pts2, openCV_F=False): img1_gray = cv.imread(img1_path, cv.IMREAD_GRAYSCALE) img2_gray = cv.imread(img2_path, cv.IMREAD_GRAYSCALE) ret_bool, rectmat1, rectmat2 = cv.stereoRectifyUncalibrated(pts1, pts2, F, (img1_gray.shape[1], img1_gray.shape[0])) print(rectmat1) rectmat1_inv = np.linalg.inv(rectmat1) rectmat2 = rectmat1_inv.dot(rectmat2) dst2 = cv.warpPerspective(img2_gray, rectmat2, (img1_gray.shape[1], img1_gray.shape[0])) out = np.concatenate([img1_gray, dst2], axis=1) plt.imshow(out) plt.show() if (img2_path == '/content/drive/My Drive/Colab Notebooks/A4/I2.jpg'): if (openCV_F == True): cv.imwrite('/content/drive/My Drive/Colab Notebooks/A4/I1-I2-openCV-rectify.png', out) else: cv.imwrite('/content/drive/My Drive/Colab Notebooks/A4/I1-I2-rectify.png', out) else: if (openCV_F == True): cv.imwrite('/content/drive/My Drive/Colab Notebooks/A4/I1-I3-openCV-rectify.png', out) else: cv.imwrite('/content/drive/My Drive/Colab Notebooks/A4/I1-I3-rectify.png', out)
def rectifyWrapper(imgL, imgR, lines1, lines2, pts1, pts2, F): # We need to apply the perspective transformation # e.g. http://www.pyimagesearch.com/2014/05/05/building-pokedex-python-opencv-perspective-warping-step-5-6/ # Transform the images so the matching horizontal lines will be horizontal # with each other between images, http://scientiatertiidimension.blogspot.ca/2013/11/playing-with-disparity.html # (hint: cv2.stereoRectifyUncalibrated, cv2.warpPerspective). imgsize = (imgL.shape[1], imgL.shape[0]) # transpose pts1 = pts1.T pts2 = pts2.T try: H1, H2 = cv2.stereoRectifyUncalibrated(pts1, pts2, F, imgsize) except cv2.error: print "cv2.error" H1, H2 = rectify_uncalibrated(lines1, lines2, pts1, pts2, F, imgsize) print "applying the custom-rectify code then as the cv2.stereoRectifyUncalibrated failed" # http://stackoverflow.com/questions/19704369/stereorectifyuncalibrated-not-accepting-same-array-as-findfundamentalmat # OpenCV Error: Assertion failed (CV_IS_MAT(_points1) && CV_IS_MAT(_points2) && ... # correct for shearing # http://scicomp.stackexchange.com/questions/2844/shearing-and-hartleys-rectification S = rectify_shearing(H1, H2, imgsize) H1 = S.dot(H1) # Init Undistort, Map (mapx / mapy) img1, map1x, map1y, img2, map2x, map2y = undistortMap(H1, H2, imgL, imgR) # Remap rimg1, rimg2 = remap(img1, map1x, map1y, img2, map2x, map2y) return rimg1, rimg2
F, inlier_mask = cv2.findFundamentalMat(best_kp1, best_kp2, cv2.FM_7POINT) inlier_mask = inlier_mask.flatten() #points within epipolar lines inlier_kp1 = best_kp1[inlier_mask == 1] inlier_kp2 = best_kp2[inlier_mask == 1] inlier_matches = best_matches[inlier_mask == 1] img3 = cv2.drawMatches(I1, kp1, I2, kp2, inlier_matches, flags=2, outImg=None) plt.imshow(img3), plt.show() thresh = 0 _, H1, H2 = cv2.stereoRectifyUncalibrated(np.float32(inlier_kp1), np.float32(inlier_kp2), F, I1gray.shape[::-1], 1) I1_rect = np.float32([[[0, 0], [I1.shape[1], 0], [I1.shape[1], I1.shape[0]], [0, I1.shape[0]]]]) warped_I1_rect = cv2.perspectiveTransform(I1_rect, H1) I2_rect = np.float32([[[0, 0], [I2.shape[1], 0], [I2.shape[1], I2.shape[0]], [0, I2.shape[0]]]]) warped_I2_rect = cv2.perspectiveTransform(I2_rect, H2) min_x_I1 = min(warped_I1_rect[0][0][0], warped_I1_rect[0][1][0], warped_I1_rect[0][2][0], warped_I1_rect[0][3][0]) min_x_I2 = min(warped_I2_rect[0][0][0], warped_I2_rect[0][1][0], warped_I2_rect[0][2][0], warped_I2_rect[0][3][0])
def match(detector_name, descriptor_name, matcher_name, image1_file, image2_file): print "\n###############################\n" print detector_name+"\t"+descriptor_name+"\t"+matcher_name # Read images image1 = cv2.imread(image1_file, cv2.CV_LOAD_IMAGE_GRAYSCALE) image2 = cv2.imread(image2_file, cv2.CV_LOAD_IMAGE_GRAYSCALE) # -- Step 1: Compute keypoints if (detector_name == "SIFT"): #configurable sift constructor -> http://docs.opencv.org/modules/nonfree/doc/feature_detection.html BEST_FEATURES = 100 #default 0 meaning get all OCTAVE_LAYERS = 5 #default 3 CONTRAST_THRESHOLD = 0.04 #default 0.04, larger -> less points EDGE_THRESHOLD = 10 #default 10, smaller -> less points SIGMA = 1.6 #default 1.6. meaning the level of gaussian blur detector = cv2.SIFT(BEST_FEATURES,OCTAVE_LAYERS,CONTRAST_THRESHOLD,EDGE_THRESHOLD,SIGMA) elif (detector_name == "SURF"): HESSIAN_THRESHOLD = 500 #larger -> less points OCTAVES = 4 #default 4 OCTAVE_LAYERS = 2 #default 2 EXTENDED = True #default true, ie use 128 descriptor otherwise 64 UPRIGHT = False#default false, ie compute orientation detector = cv2.SURF(HESSIAN_THRESHOLD,OCTAVES,OCTAVE_LAYERS,EXTENDED,UPRIGHT) else: detector = cv2.FeatureDetector_create(detector_name) t1 = time.time() keypoints1 = detector.detect(image1) t2 = time.time() print "Time to get keypoints for query image: ", str(t2 - t1) keypoints2 = detector.detect(image2) # draw sift key points with sizes and their oritentations img1 = cv2.drawKeypoints(image1,keypoints1,flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) cv2.imwrite('sift_keypoints1.jpg',img1) img2 = cv2.drawKeypoints(image2,keypoints2,flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) cv2.imwrite('sift_keypoints2.jpg',img2) # -- Step 2: Compute descriptors descriptor = cv2.DescriptorExtractor_create(descriptor_name) t1 = time.time() (keypoints1, descriptors1) = descriptor.compute(image1, keypoints1) t2 = time.time() print "Time to get descriptors for query image: ", str(t2 - t1) (keypoints2, descriptors2) = descriptor.compute(image2, keypoints2) #=============================================================================== # # -- Step 3: Matching descriptor t1 = time.time() matcher = cv2.DescriptorMatcher_create(matcher_name) bimatches = matcher.knnMatch(descriptors1, descriptors2,2) #for each feature find 2 best matches if can. #Filter out a match if the two bests are too close. We'd rather not take those points. RATIO_THREASHOLD = 0.9; # bigger -> more points remains, max = 1 ie don't discard any. matches = [] for m in bimatches: if(len(m)==2): if(m[0].distance <= RATIO_THREASHOLD*m[1].distance): matches.append(m[0]) #knnmatch automatically sort the points such that first point always has less distance else: matches.append(m) # # t2 = time.time() # # # print number of matches # print "time: ", t2-t1 # print "detector: ", detector_name, " descriptor extractor: ", descriptor_name, " matcher: ", matcher_name # print '#matches:', len(matches) #=============================================================================== t3 = time.time() print "Time to match: ", str(t3 - t1) # -- Step 4: Draw matches on image print image1.shape h1, w1 = image1.shape[:2] print w1, h1 h2, w2 = image2.shape[:2] view1 = np.zeros((max(h1, h2), w1 + w2), np.uint8) view1[:h1, :w1] = image1 view1[:h2, w1:] = image2 view1[:, :] = view1[:, :] view1[:, :] = view1[:, :] view = copy.copy(view1) view_trans_FM = copy.copy(view) view_final_match_FM = copy.copy(view) view_final_match_orig_FM = copy.copy(view) view_trans_HM = copy.copy(view) view_final_match_HM = copy.copy(view) view_final_match_orig_HM = copy.copy(view) # Draw all matches between two images for m in matches: color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) Qpt = (int(keypoints1[m.queryIdx].pt[0]), int(keypoints1[m.queryIdx].pt[1])) Tpt = (int(keypoints2[m.trainIdx].pt[0])+w1, int(keypoints2[m.trainIdx].pt[1])) cv2.line(view1, Qpt, Tpt, color, 3) cv2.circle(view1, Qpt, 10,color, 3) cv2.circle(view1, Tpt, 10,color, 3) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_all_matches.jpg", view1) dist = [m.distance for m in matches] min_dist = min(dist) avg_dist = (sum(dist) / len(dist)) print 'distance: min: %.3f' % min_dist print 'distance: mean: %.3f' % avg_dist print 'distance: max: %.3f' % max(dist) # keep only the reasonable matches # good_matches = heapq.nsmallest(20, matches, key=lambda match: match.distance) good_matches = [m for m in matches if m.distance < avg_dist*0.8] # good_matches.sort(cmp=None, key=distance, reverse=False); # sorted(good_matches, key=lambda match: match.distance) print "Number of match is: "+str(len(matches)) print "Number of good match is: "+str(len(good_matches)) src_points = [] dest_points = [] for m in good_matches: color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) Qpt = (int(keypoints1[m.queryIdx].pt[0]), int(keypoints1[m.queryIdx].pt[1])) Tpt = (int(keypoints2[m.trainIdx].pt[0]+w1), int(keypoints2[m.trainIdx].pt[1])) src_points.append(keypoints1[m.queryIdx].pt) dest_points.append(keypoints2[m.trainIdx].pt) cv2.line(view, Qpt, Tpt, color, 3) cv2.circle(view, Qpt, 10,color, 3) cv2.circle(view, Tpt, 10,color, 3) # Draw out good matches cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_good_matches.jpg", view) if len(src_points) > 8: #Compute the homography with RANSAC F, M = cv2.findFundamentalMat(np.array(src_points, dtype='float32'), np.array(dest_points, dtype='float32'), cv.CV_FM_RANSAC, 3, 0.99) #stereoRectifyUncalibrated use different format for points. src_pts = [] des_pts = [] for i in range(len(src_points)): src_pts.append(src_points[i][0]) src_pts.append(src_points[i][1]) des_pts.append(dest_points[i][0]) des_pts.append(dest_points[i][1]) r, H1, H2 = cv2.stereoRectifyUncalibrated(np.array(src_pts, dtype='float32'), np.array(des_pts, dtype='float32'), F, image1.shape,threshold=5) srcTri = np.array([(0,0), (w1,0), (w1,h1), (0,h1)],dtype='float32') srcTri = np.array([srcTri]) height, width = view.shape[:2] desTri_FM = cv2.perspectiveTransform(srcTri, H2) #Result from stereoRectifyUncalibrated if isGoodQuad(desTri_FM[0]): # if True: # //-- Draw lines between the corners (the mapped object in the scene - image_2 ) cv2.line(view_trans_FM, (int(desTri_FM[0][0][0]) + w1, int(desTri_FM[0][0][1])), (int(desTri_FM[0][1][0]) + w1, int(desTri_FM[0][1][1])), (255,255,255), 4) cv2.line(view_trans_FM, (int(desTri_FM[0][1][0]) + w1, int(desTri_FM[0][1][1])), (int(desTri_FM[0][2][0]) + w1, int(desTri_FM[0][2][1])), (255,255,255), 4) cv2.line(view_trans_FM, (int(desTri_FM[0][2][0]) + w1, int(desTri_FM[0][2][1])), (int(desTri_FM[0][3][0]) + w1, int(desTri_FM[0][3][1])), (255,255,255), 4) cv2.line(view_trans_FM, (int(desTri_FM[0][3][0]) + w1, int(desTri_FM[0][3][1])), (int(desTri_FM[0][0][0]) + w1, int(desTri_FM[0][0][1])), (255,255,255), 4) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_perspectiveTrans_FM.jpg", view_trans_FM) # Perform perspectiveTransform on all source points; dest_trans_points_FM = cv2.perspectiveTransform(np.array([src_points], dtype='float32'), H2) final_src_points = [] final_des_points = [] #filter out miss matched points for i in range(len(src_points)): des_pt = dest_points[i] trans_pt_FM = dest_trans_points_FM[0][i] if math.hypot(des_pt[0] - trans_pt_FM[0], des_pt[1] - trans_pt_FM[1]) < 200: final_src_points.append(src_points[i]) final_des_points.append(((int(trans_pt_FM[0])), int(trans_pt_FM[1]))) color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) cv2.line(view_final_match_FM, (int(src_points[i][0]), int(src_points[i][1])), ((int(trans_pt_FM[0]))+w1, int(trans_pt_FM[1])), color, 3) cv2.circle(view_final_match_FM, (int(src_points[i][0]), int(src_points[i][1])), 10,color, 3) cv2.circle(view_final_match_FM, ((int(trans_pt_FM[0]))+w1, int(trans_pt_FM[1])), 10,color, 3) #Draw original points on dest image cv2.line(view_final_match_orig_FM, (int(src_points[i][0]), int(src_points[i][1])), ((int(des_pt[0]))+w1, int(des_pt[1])), color, 3) cv2.circle(view_final_match_orig_FM, (int(src_points[i][0]), int(src_points[i][1])), 10,color, 3) cv2.circle(view_final_match_orig_FM, ((int(des_pt[0]))+w1, int(des_pt[1])), 10,color, 3) print "Fundamental Metrix Final number of matches %d"% (len(final_src_points)) if len(final_src_points) > 0: cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_FM_FinalMatches.jpg", view_final_match_FM) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_FM_FinalMatches(orig).jpg", view_final_match_orig_FM) H,mask = cv2.findHomography(np.array(src_points, dtype='float32'), np.array(dest_points, dtype='float32'), cv.CV_RANSAC) H = np.array(H, dtype='float32') dest_trans_points_HM = cv2.perspectiveTransform(np.array([src_points], dtype='float32'), H) desTri_HM = cv2.perspectiveTransform(srcTri, H) #result from homography if isGoodQuad(desTri_HM[0]): # if True: # //-- Draw lines between the corners (the mapped object in the scene - image_2 ) cv2.line(view_trans_HM, (int(desTri_HM[0][0][0]) + w1, int(desTri_HM[0][0][1])), (int(desTri_HM[0][1][0]) + w1, int(desTri_HM[0][1][1])), (0,204,0), 4) cv2.line(view_trans_HM, (int(desTri_HM[0][1][0]) + w1, int(desTri_HM[0][1][1])), (int(desTri_HM[0][2][0]) + w1, int(desTri_HM[0][2][1])), (0,204,0), 4) cv2.line(view_trans_HM, (int(desTri_HM[0][2][0]) + w1, int(desTri_HM[0][2][1])), (int(desTri_HM[0][3][0]) + w1, int(desTri_HM[0][3][1])), (0,204,0), 4) cv2.line(view_trans_HM, (int(desTri_HM[0][3][0]) + w1, int(desTri_HM[0][3][1])), (int(desTri_HM[0][0][0]) + w1, int(desTri_HM[0][0][1])), (0,204,0), 4) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_perspectiveTrans_HM.jpg", view_trans_HM) # Perform perspectiveTransform on all source points; dest_trans_points_HM = cv2.perspectiveTransform(np.array([src_points], dtype='float32'), H) final_src_points = [] final_des_points = [] #filter out miss matched points for i in range(len(src_points)): des_pt = dest_points[i] trans_pt_HM = dest_trans_points_HM[0][i] if math.hypot(des_pt[0] - trans_pt_HM[0], des_pt[1] - trans_pt_HM[1]) < 200: final_src_points.append(src_points[i]) final_des_points.append(((int(trans_pt_HM[0])), int(trans_pt_HM[1]))) color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) cv2.line(view_final_match_HM, (int(src_points[i][0]), int(src_points[i][1])), ((int(trans_pt_HM[0]))+w1, int(trans_pt_HM[1])), color, 3) cv2.circle(view_final_match_HM, (int(src_points[i][0]), int(src_points[i][1])), 10,color, 3) cv2.circle(view_final_match_HM, ((int(trans_pt_HM[0]))+w1, int(trans_pt_HM[1])), 10,color, 3) #Draw original points on dest image cv2.line(view_final_match_orig_HM, (int(src_points[i][0]), int(src_points[i][1])), ((int(des_pt[0]))+w1, int(des_pt[1])), color, 3) cv2.circle(view_final_match_orig_HM, (int(src_points[i][0]), int(src_points[i][1])), 10,color, 3) cv2.circle(view_final_match_orig_HM, ((int(des_pt[0]))+w1, int(des_pt[1])), 10,color, 3) print "Homography found Final number of matches %d"% (len(final_src_points)) if len(final_src_points) > 0: cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_HM_FinalMatches.jpg", view_final_match_HM) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_HM_FinalMatches(orig).jpg", view_final_match_orig_HM) print "\n###############################\n"
def rectify (left, right, rmin=None): # Check image dimensions if left.shape != right.shape: raise ValueError ("left/right images must have the same dimensions") h,w,d = left.shape mask = ones((h,w), dtype=uint8) # Run a SURF detector on both images detector = cv2.SURF(0.) kp1, desc1 = detector.detect(uint8(left.mean(axis=2)).copy(), mask, False) kp2, desc2 = detector.detect(uint8(right.mean(axis=2)).copy(), mask, False) desc1 = desc1.reshape ((len(kp1), -1)) desc2 = desc2.reshape ((len(kp2), -1)) # Put descriptor responses in arrays resp1 = array([kp.response for kp in kp1]) resp2 = array([kp.response for kp in kp2]) #print ("Left : Found {0} descr. MIN/AVG/MAX:STD is {1} / {2} / {3} : {4}".format (len(kp1), resp1.min(), resp1.mean(), resp1.max(), resp1.std())) #print ("Right: Found {0} descr. MIN/AVG/MAX:STD is {1} / {2} / {3} : {4}".format (len(kp2), resp2.min(), resp2.mean(), resp2.max(), resp2.std())) #drawkp (left, kp1) # We want to keep only descriptors which are slightly (by std-dev) better than average rmin1 = rmin rmin2 = rmin if rmin is None: rmin1 = resp1.mean() rmin2 = resp2.mean() iok1 = find(resp1 > rmin1) iok2 = find(resp2 > rmin2) kp1 = array(kp1)[iok1] kp2 = array(kp2)[iok2] desc1 = desc1[iok1, :].copy() desc2 = desc2[iok2, :].copy() # Match descriptors, see http://www.maths.lth.se/matematiklth/personal/solem/book.html for more info matchidx = -1 * ones ((len(desc1)), 'int') desc2t = desc2.transpose() dist_ratio = 0.6 for i in range(len(desc1)): dotprods = dot (desc1[i,:], desc2t) * 0.9999 acdp = arccos (dotprods) index = argsort (acdp) if acdp[index[0]] < dist_ratio * acdp[index[1]]: matchidx[i] = index[0] # Only keep matched descriptors kp1 = [kp1[i] for i in range(len(matchidx)) if matchidx[i] >= 0] kp2 = [kp2[matchidx[i]] for i in range(len(matchidx)) if matchidx[i] >= 0] kp1a = array ([kp.pt for kp in kp1]) kp2a = array ([kp.pt for kp in kp2]) # Rectify images ff = cv2.findFundamentalMat (kp1a, kp2a)[0] rv, h1, h2 = cv2.stereoRectifyUncalibrated (kp1a.transpose().reshape ((-1)), kp2a.transpose().reshape((-1)), ff, (h, w)) left_r = left.copy() right_r = cv2.warpPerspective (right, np.linalg.inv(np.mat(h1)) * np.mat(h2), (w,h)) return left_r, right_r
matches = sorted(matches, key=lambda x: x.distance) pts1 = [] pts2 = [] for mat in matches: idx1 = mat.queryIdx idx2 = mat.trainIdx pts1.append(kp1[idx1].pt) pts2.append(kp2[idx2].pt) pts1 = np.int32(pts1) pts2 = np.int32(pts2) #calc fundamental matrix F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS) #ロバスト推定 ret, HL, HR = cv2.stereoRectifyUncalibrated(pts1, pts2, F, (width, height)) dstL = cv2.warpPerspective(imgL, HL, (width, height)) dstR = cv2.warpPerspective(imgR, HR, (width, height)) #stereoBM stereo = cv2.StereoBM_create(16) disp = stereo.compute(dstL, dstR) orb = cv2.ORB_create() kp1, des1 = orb.detectAndCompute(dstL, None) kp2, des2 = orb.detectAndCompute(dstR, None) bf = cv2.BFMatcher(cv2.NORM_HAMMING, True) matches = bf.match(des1, des2) matches = sorted(matches, key=lambda x: x.distance)
def match(detector_name, descriptor_name, matcher_name, image1_file, image2_file): print "\n###############################\n" print detector_name # Read images image1 = cv2.imread(image1_file, cv2.CV_LOAD_IMAGE_GRAYSCALE) image2 = cv2.imread(image2_file, cv2.CV_LOAD_IMAGE_GRAYSCALE) # -- Step 1: Compute keypoints detector = cv2.FeatureDetector_create(detector_name) t1 = time.time() keypoints1 = detector.detect(image1) t2 = time.time() print "Time to get keypoints for query image: ", str(t2 - t1) keypoints2 = detector.detect(image2) # -- Step 2: Compute descriptors descriptor = cv2.DescriptorExtractor_create(descriptor_name) t1 = time.time() (keypoints1, descriptors1) = descriptor.compute(image1, keypoints1) t2 = time.time() print "Time to get descriptors for query image: ", str(t2 - t1) (keypoints2, descriptors2) = descriptor.compute(image2, keypoints2) #=============================================================================== # # -- Step 3: Matching descriptor t1 = time.time() matcher = cv2.DescriptorMatcher_create(matcher_name) matches = matcher.match(descriptors1, descriptors2) # # t2 = time.time() # # # print number of matches # print "time: ", t2-t1 # print "detector: ", detector_name, " descriptor extractor: ", descriptor_name, " matcher: ", matcher_name # print '#matches:', len(matches) #=============================================================================== t3 = time.time() print "Time to match: ", str(t3 - t1) # -- Step 4: Draw matches on image print image1.shape h1, w1 = image1.shape[:2] print w1, h1 h2, w2 = image2.shape[:2] view1 = np.zeros((max(h1, h2), w1 + w2), np.uint8) view1[:h1, :w1] = image1 view1[:h2, w1:] = image2 view1[:, :] = view1[:, :] view1[:, :] = view1[:, :] view = copy.copy(view1) view2 = copy.copy(view) view3 = copy.copy(view) view4 = copy.copy(view) # Draw all matches between two images for m in matches: color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) Qpt = (int(keypoints1[m.queryIdx].pt[0]), int(keypoints1[m.queryIdx].pt[1])) Tpt = (int(keypoints2[m.trainIdx].pt[0])+w1, int(keypoints2[m.trainIdx].pt[1])) cv2.line(view1, Qpt, Tpt, color, 3) cv2.circle(view1, Qpt, 10,color, 3) cv2.circle(view1, Tpt, 10,color, 3) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_all_matches.jpg", view1) dist = [m.distance for m in matches] min_dist = min(dist) avg_dist = (sum(dist) / len(dist)) print 'distance: min: %.3f' % min_dist print 'distance: mean: %.3f' % avg_dist print 'distance: max: %.3f' % max(dist) # threshold: half the mean # thres_dist = (sum(dist) / len(dist)) * 0.5 # keep only the reasonable matches # good_matches = heapq.nsmallest(20, matches, key=lambda match: match.distance) good_matches = [m for m in matches if m.distance < avg_dist*0.8] # good_matches.sort(cmp=None, key=distance, reverse=False); # sorted(good_matches, key=lambda match: match.distance) print "Number of match is: "+str(len(matches)) print "Number of good match is: "+str(len(good_matches)) src_points = [] dest_points = [] for m in good_matches: color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) Qpt = (int(keypoints1[m.queryIdx].pt[0]), int(keypoints1[m.queryIdx].pt[1])) Tpt = (int(keypoints2[m.trainIdx].pt[0]), int(keypoints2[m.trainIdx].pt[1])) src_points.append(keypoints1[m.queryIdx].pt) dest_points.append(keypoints2[m.trainIdx].pt) cv2.line(view, Qpt, (Tpt[0]+w1, Tpt[1]), color, 3) cv2.circle(view, Qpt, 10,color, 3) cv2.circle(view, (Tpt[0]+w1, Tpt[1]), 10,color, 3) # Draw out good matches cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_good_matches.jpg", view) #Compute the homography with RANSAC t1 = time.time() H,mask = cv2.findHomography(np.array(src_points, dtype='float32'), np.array(dest_points, dtype='float32'), cv.CV_RANSAC) t3 = time.time() print "Time to perform geoverification: ", str(t3 - t1) F, M = cv2.findFundamentalMat(np.array(src_points, dtype='float32'), np.array(dest_points, dtype='float32'), cv.CV_FM_RANSAC, 3, 0.99) src_pts = [] des_pts = [] for i in range(len(src_points)): src_pts.append(src_points[i][0]) src_pts.append(src_points[i][1]) des_pts.append(dest_points[i][0]) des_pts.append(dest_points[i][1]) r, H1, H2 = cv2.stereoRectifyUncalibrated(np.array(src_pts, dtype='float32'), np.array(des_pts, dtype='float32'), F, image1.shape,threshold=5) # r, H1, H2 = cv2.stereoRectifyUncalibrated(np.array((1,2,2,3,3,4,4,5), dtype='float32'), np.array((1,2,2,3,3,4,4,5), dtype='float32'), F, (2,2)) H = np.array(H, dtype='float32') srcTri = np.array([(0,0), (w1,0), (w1,h1), (0,h1)],dtype='float32') srcTri = np.array([srcTri]) height, width = view.shape[:2] desTri = cv2.perspectiveTransform(srcTri, H2) # //-- Draw lines between the corners (the mapped object in the scene - image_2 ) cv2.line(view2, (int(desTri[0][0][0]) + w1, int(desTri[0][0][1])), (int(desTri[0][1][0]) + w1, int(desTri[0][1][1])), (255,255,255), 4) cv2.line(view2, (int(desTri[0][1][0]) + w1, int(desTri[0][1][1])), (int(desTri[0][2][0]) + w1, int(desTri[0][2][1])), (255,255,255), 4) cv2.line(view2, (int(desTri[0][2][0]) + w1, int(desTri[0][2][1])), (int(desTri[0][3][0]) + w1, int(desTri[0][3][1])), (255,255,255), 4) cv2.line(view2, (int(desTri[0][3][0]) + w1, int(desTri[0][3][1])), (int(desTri[0][0][0]) + w1, int(desTri[0][0][1])), (255,255,255), 4) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_perspectiveTrans.jpg", view2) # Perform perspectiveTransform on all source points; dest_trans_points = cv2.perspectiveTransform(np.array([src_points], dtype='float32'), H2) final_src_points = [] final_des_points = [] #filter out miss matched points for i in range(len(src_points)): des_pt = dest_points[i] trans_pt = dest_trans_points[0][i] if math.hypot(des_pt[0] - trans_pt[0], des_pt[1] - trans_pt[1]) < 100: final_src_points.append(src_points[i]) final_des_points.append(((int(trans_pt[0])), int(trans_pt[1]))) color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) cv2.line(view3, (int(src_points[i][0]), int(src_points[i][1])), ((int(trans_pt[0]))+w1, int(trans_pt[1])), color, 3) cv2.circle(view3, (int(src_points[i][0]), int(src_points[i][1])), 10,color, 3) cv2.circle(view3, ((int(trans_pt[0]))+w1, int(trans_pt[1])), 10,color, 3) #Draw original points on dest image cv2.line(view4, (int(src_points[i][0]), int(src_points[i][1])), ((int(des_pt[0]))+w1, int(des_pt[1])), color, 3) cv2.circle(view4, (int(src_points[i][0]), int(src_points[i][1])), 10,color, 3) cv2.circle(view4, ((int(des_pt[0]))+w1, int(des_pt[1])), 10,color, 3) print "Final number of matches %d"% (len(final_src_points)) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_FinalMatches.jpg", view3) cv2.imwrite(detector_name+"_"+descriptor_name+"_"+matcher_name+"_FinalMatches(orig).jpg", view4) print "\n###############################\n"
distCoeffs1 = distcoeffs1, cameraMatrix2 = cameramatrix2, distCoeffs2 = distcoeffs2, R = R, T = T, E = E, F = F, flags = sum(allflags)) rms.append(out[0]) # <codecell> plt.plot((rms)) # plt.ylim(ymax = 4) plt.show() # <codecell> cv2.stereoRectifyUncalibrated() # <codecell> R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(cameraMatrix1 = cameramatrix1, distCoeffs1 = distcoeffs1, cameraMatrix2 = cameramatrix2, distCoeffs2 = distcoeffs2, imageSize = (image_width, image_height), R = R, T = T, flags = 0 * cv2.CALIB_ZERO_DISPARITY, alpha = 1, newImageSize = (1 * image_width, 1 * image_height) ) # <codecell>
def main(): img1 = cv2.imread('./Dataset 2/im0.png') img2 = cv2.imread('./Dataset 2/im1.png') # # Scale the image # scale_percent = 10 # percent of original size # width = int(img1.shape[1] * scale_percent / 100) # height = int(img1.shape[0] * scale_percent / 100) # dim = (width, height) # img1 = cv2.resize(img1, dim, interpolation = cv2.INTER_AREA) # img2 = cv2.resize(img2, dim, interpolation = cv2.INTER_AREA) h1, w1, ch1 = img1.shape h2, w2, ch2 = img2.shape kp1, kp2, good = detect_feature(img1, img2) feature_1 = [] feature_2 = [] for i, match in enumerate(good): feature_1.append(kp1[match.queryIdx].pt) feature_2.append(kp2[match.trainIdx].pt) # Only for testing the result pts1 = np.int32(feature_1) pts2 = np.int32(feature_2) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC) Best_F_matrix, new_feature_1, new_feature_2 = estimate_fundamental_matrix( feature_1, feature_2) # print(Best_F_matrix) new_feature_1 = np.int32(new_feature_1) new_feature_2 = np.int32(new_feature_2) E_matrix = essential_matrix(Best_F_matrix, K1) R, T = extract_camera_pose(E_matrix, K1) # print(R) # print(T) H = [] I = np.array([0, 0, 0, 1]) for i, j in zip(R, T): h = np.hstack((i, j)) h = np.vstack((h, I)) H.append(h) # print('H:\n', H) _, H1, H2 = cv2.stereoRectifyUncalibrated(np.float32(new_feature_1), np.float32(new_feature_2), Best_F_matrix, imgSize=(w1, h1)) # print("H1:\n", H1) # print("H2:\n",H2) # Store the output data in the text file file = open('./output/dataset2_output_data.txt', 'w') file.write('*' * 50 + '\n' + '(Acquired by the inbuilt function) Foundatmental matrix' + '\n') file.write(str(F) + '\n') file.write('*' * 50 + '\n' + 'Estimated foundatmental matrix' + '\n') file.write(str(Best_F_matrix) + '\n') file.write('*' * 50 + '\n' + 'Essential matrix' + '\n') file.write(str(E_matrix) + '\n') file.write('*' * 50 + '\n' + 'Rotation vector' + '\n') file.write(str(R) + '\n') file.write('*' * 50 + '\n' + 'Translation vector' + '\n') file.write(str(T) + '\n') file.write('*' * 50 + '\n' + 'Homography matrix (H1)' + '\n') file.write(str(H) + '\n') file.write('*' * 50 + '\n' + 'Homography matrix img1' + '\n') file.write(str(H1) + '\n') file.write('*' * 50 + '\n' + 'Homography matrix img2' + '\n') file.write(str(H2) + '\n') file.write('*' * 50 + '\n') file.close() img1_rectified = cv2.warpPerspective(img1, H1, (w1, h1)) img2_rectified = cv2.warpPerspective(img2, H2, (w2, h2)) img1_res, img2_res = rectification(img1_rectified, img2_rectified) res = np.concatenate((img1_res, img2_res), axis=1) cv2.imshow("Rectification", res) img1_rectified = cv2.warpPerspective(img1, H1, (w1, h1)) img2_rectified = cv2.warpPerspective(img2, H2, (w2, h2)) disparity_map = get_disparity_map(img1_rectified, img2_rectified) disparity_map_gray = None disparity_map_gray = cv2.normalize(disparity_map, disparity_map_gray, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) cv2.imshow('disparity_gray', disparity_map_gray) depth_map = get_depth_map(disparity_map_gray) disparity_map_heat = None disparity_map_heat = cv2.normalize(disparity_map, disparity_map_heat, alpha=vmin, beta=vmax, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) disparity_map_heat = cv2.applyColorMap(disparity_map_heat, cv2.COLORMAP_JET) cv2.imshow("disparity_heat", disparity_map_heat) depth_map_gray = None depth_map_gray = cv2.normalize(depth_map, depth_map_gray, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) cv2.imshow('depth_gray', depth_map_gray) depth_map_heat = cv2.applyColorMap(depth_map_gray, cv2.COLORMAP_JET) cv2.imshow("depth_heat", depth_map_heat) cv2.imwrite('./output/data_2_Rectification.jpg', res) cv2.imwrite('./output/data_2_disparity_gray.jpg', disparity_map_gray) cv2.imwrite('./output/data_2_disparity_heat.jpg', disparity_map_heat) cv2.imwrite('./output/data_2_depth_gray.jpg', depth_map) cv2.imwrite('./output/data_2_depth_heat.jpg', depth_map_heat) cv2.waitKey(0) cv2.destroyAllWindows()
return img1color, img2color lines1 = cv.computeCorrespondEpilines( pts2.reshape(-1, 1, 2), 2, fundamental_matrix) lines1 = lines1.reshape(-1, 3) img5, img6 = drawlines(img1, img2, lines1, pts1, pts2) lines2 = cv.computeCorrespondEpilines( pts1.reshape(-1, 1, 2), 1, fundamental_matrix) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(img2, img1, lines2, pts2, pts1) h1, w1 = img1.shape h2, w2 = img2.shape _, H1, H2 = cv.stereoRectifyUncalibrated( np.float32(pts1), np.float32(pts2), fundamental_matrix, imgSize=(w1, h1) ) img1_rectified = cv.warpPerspective(img1, H1, (w1, h1)) img2_rectified = cv.warpPerspective(img2, H2, (w2, h2)) block_size = 15 min_disp = -96 max_disp = 112 num_disp = max_disp - min_disp uniquenessRatio = 5 speckleWindowSize = 0
# space, which explains the suffix "uncalibrated". Another related difference # from cv.stereoRectify is that the function outputs not the rectification # transformations in the object (3D) space, but the planar perspective # transformations encoded by the homography matrices `H1` and `H2`. The # function implements the algorithm [Hartley99]. # # ### Note # While the algorithm does not need to know the intrinsic parameters of the # cameras, it heavily depends on the epipolar geometry. Therefore, if the # camera lenses have a significant distortion, it would be better to correct # it before computing the fundamental matrix and calling this function. For # example, distortion coefficients can be estimated for each head of stereo # camera separately by using cv.calibrateCamera. Then, the images can be # corrected using cv.undistort, or just the point coordinates can be corrected # with cv.undistortPoints. ret, Hl, Hr = cv.stereoRectifyUncalibrated(new_left[:, :, 0:2], new_right[:, :, 0:2], F, (C, R)) Hl/=Hl[2,2] Hr/=Hr[2,2] Hl[0,2]+=250 # Printing the Homographies print('Left Homography: \n', Hl) print('The Right Homography: \n',Hr) # Rectifying the Images: # WARPPERSPECTIVE Applies a perspective transformation to an image # # dst = cv.warpPerspective(src, M) # dst = cv.warpPerspective(src, M, 'OptionName',optionValue, ...) #
def main(): Parser = argparse.ArgumentParser() Parser.add_argument('--dataset', default='1', help='Dataset type , Default: 1') Args = Parser.parse_args() dataset = int(Args.dataset) if dataset == 1: SavePath = './Output/Dataset1/' d_thresh = 100000 if dataset == 2: SavePath = './Output/Dataset2/' d_thresh = 90000 if dataset == 3: SavePath = './Output/Dataset3/' d_thresh = 200000 images, K1, K2, params = readData(dataset, BasePath="../Data/Project 3/") foldercheck(SavePath) im1, im2 = images im1, im2 = rgb(rescale(im1, 30)), rgb(rescale(im2, 30)) h1, w1 = im1.shape[:2] h2, w2 = im2.shape[:2] pts1, pts2, im_matches = SIFTpoints(im1, im2) print('SIFT points Detected : ', len(pts1)) data = (pts1, pts2) F, inlier_mask = FundamentalMatrix(data, s=8, thresh=0.001, n_iterations=2000) pts1_ = pts1[inlier_mask == 1] pts2_ = pts2[inlier_mask == 1] E = EssentialMatrix(K1, K2, F) print('Inliers SIFT points : ', len(pts1_)) R, C, x3D = recoverPose(E, pts1_, pts2_, K1, K2) l1 = cv2.computeCorrespondEpilines(pts2_.reshape(-1, 1, 2), 2, F) im2_epilines, _ = drawEpilines(im1, im2, l1[:10], pts1_, pts2_) l2 = cv2.computeCorrespondEpilines(pts1_.reshape(-1, 1, 2), 1, F) im1_epilines, _ = drawEpilines(im2, im1, l2[:10], pts2_, pts1_) out = np.hstack((im1_epilines, im2_epilines)) cv2.imwrite(SavePath + 'epilinesImage1.png', out) ret, H1, H2 = cv2.stereoRectifyUncalibrated(np.float32(pts1_), np.float32(pts2_), F, imgSize=(w1, h1)) im1_rectified = cv2.warpPerspective(im1, H1, (w1, h1)) im2_rectified = cv2.warpPerspective(im2, H2, (w2, h2)) out = np.hstack((im1_rectified, im2_rectified)) cv2.imwrite(SavePath + 'rectifiedImage.png', out) dst1 = cv2.perspectiveTransform(pts1_.reshape(-1, 1, 2), H1).squeeze() dst2 = cv2.perspectiveTransform(pts2_.reshape(-1, 1, 2), H2).squeeze() lines1_ = epiLines(pts2_, 2, F, w2) warpedlines1 = warpEpilines(lines1_, H1) lines2_ = epiLines(pts1_, 1, F, w2) warpedlines2 = warpEpilines(lines2_, H2) im1_print = drawLines(im1_rectified, warpedlines1[:10], dst1[:10]) im2_print = drawLines(im2_rectified, warpedlines2[:10], dst2[:10]) out = np.hstack((im1_print, im2_print)) cv2.imwrite(SavePath + 'epilines_rectifiedImage.png', out) imL, imR = gray(im1_rectified), gray(im2_rectified) disparityMap = DisparityMap(imL, imR, warpedlines1, warpedlines2, win_size=10, searchRange=100) np.save(SavePath + 'disparityMap.npy', disparityMap) # disparity_map_print = cv2.normalize(disparityMap, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F).astype(np.uint8) plt.figure(figsize=(10, 10)) plt.imshow(disparityMap, cmap=plt.cm.RdBu, interpolation='bilinear') plt.savefig(SavePath + ' disparityMap.png') plt.imshow(disparityMap, cmap='gray', interpolation='bilinear') plt.savefig(SavePath + ' disparityMap_gray.png') baseline = params[1] f = K1[0, 0] depthMap = (baseline * f) / (disparityMap + 1e-15) depthMap[depthMap > d_thresh] = d_thresh depthMap = np.uint8(depthMap * 255 / np.max(depthMap)) plt.figure(figsize=(10, 10)) plt.imshow(depthMap, cmap=plt.cm.RdBu, interpolation='bilinear') plt.savefig(SavePath + ' depthMap.png') plt.imshow(depthMap, cmap='gray', interpolation='bilinear') plt.savefig(SavePath + ' depthMap_gray.png')
def main(): # Toda a parte da implementação do SIFT eu peguei do usuário Bilou563 nos fóruns da OpenCV # https://answers.opencv.org/question/90742/opencv-depth-map-from-uncalibrated-stereo-system/ # Acesso em 06/05/2019 print("Carregando as imagens") img1 = cv2.imread('data/FurukawaPonce/MorpheusL.jpg', cv2.CV_8UC1) #queryimage # left image img2 = cv2.imread('data/FurukawaPonce/MorpheusR.jpg', cv2.CV_8UC1) #trainimage # right image # Ajustando o tamanho de img1 para que as imagens tenham o mesmo tamanho (necessário) img1 = img1[:, 0: 1300] # Crop de uma parte que não tem nada (eu vi antes na imagem) # E colocando no mesmo aspect ratio de img2 img1 = cv2.resize(img1, img2.shape) # Colocando no mesmo tamanho de img2 print("Encontrando pontos correspondentes com SIFT") #Obtainment of the correspondent point with SIFT sift = cv2.xfeatures2d.SIFT_create() ###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) print( "Calculando a matriz fundamental com base nos pontos correspondentes") #Computation of the fundamental matrix F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS) # Obtainment of the rectification matrix and use of the warpPerspective to transform them... pts1 = pts1[:, :][mask.ravel() == 1] pts2 = pts2[:, :][mask.ravel() == 1] pts1 = np.int32(pts1) pts2 = np.int32(pts2) p1fNew = pts1.reshape((pts1.shape[0] * 2, 1)) p2fNew = pts2.reshape((pts2.shape[0] * 2, 1)) print("Retificando as imagens com base na matriz fundamental") retBool, rectmat1, rectmat2 = cv2.stereoRectifyUncalibrated( p1fNew, p2fNew, F, img1.shape) dst11 = cv2.warpPerspective(img1, rectmat1, img1.shape, cv2.BORDER_ISOLATED) dst22 = cv2.warpPerspective(img2, rectmat2, img2.shape, cv2.BORDER_ISOLATED) print("Gravando as imagens retificadas") # Gravando as imagens retificadas cv2.imwrite('data/output/rectifiedMorpheusL.png', dst11) cv2.imwrite('data/output/rectifiedMorpheusR.png', dst22) print("Calculando a disparidade") disp = stereo.compute(dst11.astype(np.uint8), dst22.astype( np.uint8)).astype(np.float32) / 16 plt_show(disp, "data/output/dispMorpheus.png") # Histograma que usei para achar os valores 60 e -60 usados # para criar disp_filtered logo abaixo # É uma tentativa de remover os pontos que ele calculou a # disparidade errado/não encontrou match # plt.hist(disp.ravel(), 40) # plt.title("Histogram with 'auto' bins") # hist = plt.gcf() # hist.savefig("histMorpheus.png") # plt.show() disp_filtered = np.where(disp > 60, np.amin(disp), disp) disp_filtered = np.where(disp_filtered < -60, -60, disp) plt_show(disp_filtered, "data/output/dispMorpheusFiltered.png") ######################################### ######## Cálculo da profundidade ######## ######################################### print("Carregando os parâmetros de calibração") # Carregando os parâmetros de calibração # MorpheusL imL_calib = open('data/FurukawaPonce/MorpheusL.txt', 'r') texto = imL_calib.read() start_f1 = texto.find('fc = ') end_f1 = texto.find(']', start_f1) f1 = np.fromstring(texto[start_f1 + 6:end_f1].replace(';', ' '), dtype=float, sep=' ') start_c1 = texto.find('cc = ') end_c1 = texto.find(']', start_c1) c1 = np.fromstring(texto[start_c1 + 6:end_c1].replace(';', ' '), dtype=float, sep=' ') start_alpha = texto.find('alpha_c = ') end_alpha = texto.find(';', start_alpha) alpha = float(texto[start_alpha + 10:end_alpha]) start_R = texto.find('R = ') end_R = texto.find(']', start_R) R1 = np.fromstring(texto[start_R + 5:end_R].replace(',', ' ').replace(';', ' '), dtype=float, sep=' ') R1.shape = (3, 3) start_Tc = texto.find('Tc = ') end_Tc = texto.find(']', start_Tc) Tc1 = np.fromstring(texto[start_Tc + 6:end_Tc].replace(';', ' '), dtype=float, sep=' ') Tc1.shape = (3, 1) matrix1 = np.array([[f1[0], alpha, c1[0]], [0, f1[1], c1[1]], [0, 0, 1]], dtype=float) # Compensando a matriz de intrínsecos pelo resize da MorpheusL de acordo com: # https://dsp.stackexchange.com/a/6098 # Acesso em 06/05/2019 scaling_compensation = np.array([[12 / 13, 0, 1 / 26], [0, 12 / 13, 1 / 26], [0, 0, 1]]) matrix1 = scaling_compensation @ matrix1 # MorpheusR imR_calib = open('data/FurukawaPonce/MorpheusR.txt', 'r') texto = imR_calib.read() start_f2 = texto.find('fc = ') end_f2 = texto.find(']', start_f2) f2 = np.fromstring(texto[start_f2 + 6:end_f2].replace(';', ' '), dtype=float, sep=' ') start_c2 = texto.find('cc = ') end_c2 = texto.find(']', start_c2) c2 = np.fromstring(texto[start_c2 + 6:end_c2].replace(';', ' '), dtype=float, sep=' ') start_alpha = texto.find('alpha_c = ') end_alpha = texto.find(';', start_alpha) alpha = float(texto[start_alpha + 10:end_alpha]) start_R = texto.find('R = ') end_R = texto.find(']', start_R) R2 = np.fromstring(texto[start_R + 5:end_R].replace(',', ' ').replace(';', ' '), dtype=float, sep=' ') R2.shape = (3, 3) start_Tc = texto.find( 'Tc_8 = ') # Por alguma razão está como 'Tc_8' nessa imagem end_Tc = texto.find(']', start_Tc) Tc2 = np.fromstring(texto[start_Tc + 8:end_Tc].replace(';', ' '), dtype=float, sep=' ') Tc2.shape = (3, 1) matrix2 = np.array([[f2[0], alpha, c1[0]], [0, f2[1], c2[1]], [0, 0, 1]], dtype=float) # Ajustando parâmetros rvec1, _ = cv2.Rodrigues(R1) rvec2, _ = cv2.Rodrigues(R2) Tc1.shape = (1, 3) Tc2.shape = (1, 3) # Calculando o deslocamento e rotação relativos entre as câmeras rvec3, tvec3, _, _, _, _, _, _, _, _, = cv2.composeRT( rvec1.ravel(), Tc1.ravel(), rvec2.ravel(), Tc2.ravel()) print("Calculando profundidade") focal_length = np.mean([np.mean(f1), np.mean(f2)]) doffs = c2[0] - c1[0] # Diferença no eixo x entre os pontos principais baseline = np.linalg.norm(tvec3) # Distância entre as câmeras disp2depth_factor = baseline * focal_length # Disparidade em mm(?) # Tem algum fator errado aqui porque está muito grande depth = np.zeros(disp_filtered.shape, dtype=np.uint8) depth = disp2depth_factor / (disp_filtered + doffs) # plt_show(depth, "depthMorpheus_mm.png") furthest = disp2depth_factor / ( np.amin(disp_filtered[disp_filtered > -60]) + doffs) depth = disp2depth_factor / (disp_filtered + doffs) # Normalizando a profundidade de acordo com o roteiro depth[disp_filtered > -60] = np.floor( (disp2depth_factor / (disp_filtered[disp_filtered > -60] + doffs)) * 254 / furthest) depth[disp_filtered <= -60] = 255 plt_show(depth, "data/output/depthMorpheus.png")
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) pp1 = pts1.reshape((pts1.shape[0] * 2, 1)) pp2 = pts2.reshape((pts2.shape[0] * 2, 1)) size = img1.shape[1], img1.shape[0] suc, h1, h2 = cv2.stereoRectifyUncalibrated(pp1, pp2, F, size) R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(M, dis, M, dis, size, R, T) print validPixROI1, validPixROI2 cv2.imwrite("img2p.png", img3) cv2.imwrite("img1p.png", img5) img5Un = cv2.warpPerspective(img5, h1, size) img3Un = cv2.warpPerspective(img3, h2, size) cv2.imwrite("img2r Un.png", img3Un) cv2.imwrite("img1r Un.png", img5Un) img1 = cv2.imread('l.jpg',-1) img2 = cv2.imread('r.jpg',-1)
lines2 = lines2.reshape(-1, 3) img3, img4 = hc.drawlines(img2, img1, lines2, pts2, pts1) fig = plt.figure(3) axarr = fig.subplots(1, 2) axarr[0].imshow(img5, cmap='gray') axarr[1].imshow(img3, cmap='gray') plt.show() # rectification pts1 = np.int32(pts1) pts2 = np.int32(pts2) p1fNew = pts1.reshape((pts1.shape[0] * 2, 1)) p2fNew = pts2.reshape((pts2.shape[0] * 2, 1)) retBool, rectmat1, rectmat2 = cv2.stereoRectifyUncalibrated( p1fNew, p2fNew, F, img1.shape[::-1]) dst11 = cv2.warpPerspective(img1, rectmat1, img1.shape[::-1]) dst22 = cv2.warpPerspective(img2, rectmat2, img1.shape[::-1]) fig = plt.figure(4) axarr = fig.subplots(1, 2) axarr[0].imshow(dst11, cmap='gray') axarr[1].imshow(dst22, cmap='gray') plt.show() #calculation of the disparity stereoMatcher = cv2.StereoBM_create() stereoMatcher.setMinDisparity(0) stereoMatcher.setNumDisparities(128) stereoMatcher.setBlockSize(5)
def match_stereo(self, img1, img2, img1_view, img2_view, thrs = 500, ratio_thrs = 0.5): gray1 = cv2.cvtColor(img1, cv.CV_BGR2GRAY) gray2 = cv2.cvtColor(img2, cv.CV_BGR2GRAY) surf = cv2.SURF(thrs, 1, 4) kp1, d1 = surf.detect(gray1, None, False) kp2, d2 = surf.detect(gray2, None, False) d1.shape = (-1, surf.descriptorSize()) d2.shape = (-1, surf.descriptorSize()) print len(kp1), len(kp2) m = match(d1, d2, ratio_thrs) #p1 = [p.pt for p in kp1] #p2 = [p.pt for p in kp2] #m = local_match(p1, p2, d1, d2, max_dist = 5.0 / pixel_extent, min_neigh = 5, r_threshold = ratio_thrs) pairs = np.float32( [(kp1[i].pt, kp2[j].pt) for i, j in m] ) mp1, mp2 = pairs[:,0].copy(), pairs[:,1].copy() ''' for (x1, y1), (x2, y2) in np.int32(zip(mp1, mp1)): cv.circle(img1, (x1, y1), 2, (0, 255, 0)) cv.circle(img2, (x2, y2), 2, (0, 255, 0)) cv.line(img1, (x1, y1), (x2, y2), (0, 255, 0)) cv.line(img2, (x1, y1), (x2, y2), (0, 255, 0)) self.img_flip[] ''' F, status = cv2.findFundamentalMat(mp1, mp2, cv2.FM_RANSAC, 5.0) status = status.ravel() != 0 print '%d / %d' % (sum(status), len(status)) mp1, mp2 = mp1[status], mp2[status] rectified_size = (800, 800) retval, H1, H2 = cv2.stereoRectifyUncalibrated(mp1.reshape(1, -1, 2), mp2.reshape(1, -1, 2), F, rectified_size) gH1 = np.dot(H1, img1_view) gH2 = np.dot(H2, img2_view) mp1 = cv2.perspectiveTransform(mp1.reshape(1, -1, 2), H1) mp2 = cv2.perspectiveTransform(mp2.reshape(1, -1, 2), H2) d = mp1[0,:,0]-mp2[0,:,0] def draw_vis(img, H, size): return cv2.warpPerspective(img, H, size) vis1 = draw_vis(self.frames[0].lods[0], gH1, rectified_size) vis2 = draw_vis(self.frames[1].lods[0], gH2, rectified_size) anaglyph = vis2.copy() anaglyph[..., 2] = vis1[..., 2] #e1 = cv2.canny(cv2.cvtColor(vis1, cv.CV_BGR2GRAY), 100, 200) #e2 = cv2.canny(cv2.cvtColor(vis2, cv.CV_BGR2GRAY), 100, 200) #edge_anaglyph = np.dstack([e2, e2, e1]) #anaglyph = np.maximum(anaglyph, edge_anaglyph) print 'stereo matching...' disp = stereo.calc_disparity(vis1, vis2, d.min(), d.max()) fnbase = '%02d_' % self.shot_idx print 'saving ply...' verts = np.zeros(disp.shape + (3,), np.float32) verts[...,1], verts[...,0] = np.ogrid[ :rectified_size[1], :rectified_size[0] ] verts[...,2] = disp*4 verts *= 0.1 write_ply_bin(fnbase+'cloud.ply', verts, cv2.cvtColor(vis1, cv.CV_BGR2RGB)) vis_disp = disp.copy() vis_disp -= d.min() vis_disp /= np.percentile(vis_disp, 99) vis_disp = np.uint8(np.clip(vis_disp, 0, 1)*255) cv2.imshow('disp', vis_disp) cv2.imshow('anaglyph', anaglyph) cv2.imwrite(fnbase+'l.png', vis1) cv2.imwrite(fnbase+'r.png', vis2) #cv2.imwrite(fnbase+'anaglyph.bmp', anaglyph) #cv2.imwrite(fnbase+'disp.bmp', vis_disp) #cv2.imwrite(fnbase+'small.bmp', self.cur_preview) self.shot_idx += 1