def visualize_epipolar_lines(self, img1, img2, p1, p2, E, save_path): """ This function visualizes the epipolar lines img1, img2: are the two images p1, p2: are the good keypoints E: Essential matrix save_path: destination to save the visualization image """ # get fundamental matrix F, mask_fdm = cv2.findFundamentalMat(p1, p2, cv2.RANSAC) p1_selected = p1[mask_fdm.ravel() == 1] p2_selected = p2[mask_fdm.ravel() == 1] # draw lines lines1 = cv2.computeCorrespondEpilines(p2_selected.reshape(-1, 1, 2), 2, F).reshape(-1, 3) img5, _ = self.drawlines(img1, img2, lines1, p1_selected, p2_selected, 100) lines2 = cv2.computeCorrespondEpilines(p1_selected.reshape(-1, 1, 2), 1, F).reshape(-1, 3) img3, _ = self.drawlines(img2, img1, lines2, p2_selected, p1_selected, 100) canvas = np.concatenate((img5, img3), axis=1) cv2.imwrite(save_path, canvas)
def draw_epipolar_lines(F, img1, img2, pts1, pts2, display=ct.DONT_DISPLAY_PLOT): # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv.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 = cv.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(img2, img1, lines2, pts2, pts1) im.write(ct.POSE_WRITE_PATH + "epipolar_lines1.jpg", img5) im.write(ct.POSE_WRITE_PATH + "epipolar_lines2.jpg", img3) if display: plt.subplot(121), plt.imshow(img5) plt.subplot(122), plt.imshow(img3) plt.show()
def drawEpilinesAndMatches(imgL, imgR, pts1, pts2, kp1, kp2, good): # 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) img3_L = imgL img3_R = imgR img3_L, img3_R = drawlines(img3_L, img3_R, lines1, pts1, pts2) fig = plt.figure() plt.subplot(121) plt.imshow(imgL), plt.title( 'Input L (no lines should be written on these variables?)') plt.subplot(122) plt.imshow(imgR), plt.title( 'Input R (no lines should be written on these variables?)') plt.show() # 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) img4_L = imgL img4_R = imgR img4_L, img4_R = drawlines(img4_L, img4_R, lines2, pts2, pts1) # cv2.drawMatchesKnn expects list of lists as matches. # http://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_feature2d/py_matcher/py_matcher.html imgDummy = np.zeros((1, 1)) img5 = cv2.drawMatches(imgL, kp1, imgR, kp2, good[:10], imgDummy) return img3_L, img3_R, img4_L, img4_R, img5, lines1, lines2
def draw_epipolar_lines(img1, img2, corrs, F): pts1, pts2 = [], [] dists = [] for i in range(corrs.shape[0]): x_1, y_1, x_2, y_2 = corrs[i] pts1.append((y_1, x_1)) pts2.append((y_2, x_2)) pts1 = np.array(pts1) pts2 = np.array(pts2) img1 = (img1.copy()*256.).astype(np.uint8) img2 = (img2.copy()*256.).astype(np.uint8) lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F) lines1 = lines1.reshape(-1,3) img1_out,img2_out = drawlines(img1,img2,lines1,pts1,pts2) lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F) lines2 = lines2.reshape(-1,3) img4_out,img3_out = drawlines(img2,img1,lines2,pts2,pts1) img1_out = cv2.cvtColor(img1_out, cv2.COLOR_BGR2RGB) img2_out = cv2.cvtColor(img2_out, cv2.COLOR_BGR2RGB) img3_out = cv2.cvtColor(img3_out, cv2.COLOR_BGR2RGB) img4_out = cv2.cvtColor(img4_out, cv2.COLOR_BGR2RGB) return img1_out, img2_out, img3_out, img4_out
def compute_distances_kpts_to_epilines(points_i, points_j, F: np.array) -> np.array: if F is None: return (np.zeros((points_i.shape[0], 2)) + np.inf) # Epipolar lines in image I of the points in image J lines_i = cv2.computeCorrespondEpilines(points_j.reshape(-1, 1, 2), 2, F).reshape(-1, 3) # Epipolar lines in image J of the points in image I lines_j = cv2.computeCorrespondEpilines(points_i.reshape(-1, 1, 2), 1, F).reshape(-1, 3) dist = [] for k in range(points_i.shape[0]): # Params for image i xi, yi = points_i[k] ai, bi, ci = lines_i[k] # Params for image j xj, yj = points_j[k] aj, bj, cj = lines_j[k] di = np.abs(ai * xi + bi * yi + ci) / np.sqrt(ai * ai + bi * bi) dj = np.abs(aj * xj + bj * yj + cj) / np.sqrt(aj * aj + bj * bj) dist.append((di, dj)) dist = np.array(dist) return dist
def draw_epipolar_lines(self, feat_mode="SURF"): """Draws epipolar lines This method computes and draws the epipolar lines of the two loaded images. :param feat_mode: whether to use rich descriptors for feature matching ("surf") or optic flow ("flow") """ self._extract_keypoints(feat_mode) self._find_fundamental_matrix() # Find epilines corresponding to points in right image (second image) # and drawing its lines on left image pts2re = self.match_pts2.reshape(-1, 1, 2) lines1 = cv2.computeCorrespondEpilines(pts2re, 2, self.F) lines1 = lines1.reshape(-1, 3) img3, img4 = self._draw_epipolar_lines_helper(self.img1, self.img2, lines1, self.match_pts1, self.match_pts2) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image pts1re = self.match_pts1.reshape(-1, 1, 2) lines2 = cv2.computeCorrespondEpilines(pts1re, 1, self.F) lines2 = lines2.reshape(-1, 3) img1, img2 = self._draw_epipolar_lines_helper(self.img2, self.img1, lines2, self.match_pts2, self.match_pts1) cv2.imshow("left", img1) cv2.imshow("right", img3) cv2.waitKey()
def DrawCorrespondEpilines(self, src, dst, pts1, pts2, F): """Draw Correspond Epilines on the image by Fundamental matrix Parameters ---------- src : ndarray A HxW(x3) matrix of opencv image dst : ndarray A HxW(x3) matrix of opencv image pts1: List of cv2.KeyPoint keypoints matched of src image pts1: List of cv2.KeyPoint keypoints matched of dst image F: ndarray A 3x3 Matrix of Fundamental Matrix Returns ------- draw: ndarray A HxW matrix of opencv image """ lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) img1 = drawlines(src, lines1, pts1, pts2) lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img2 = drawlines(dst, lines2, pts2, pts1) return img1, img2
def draw_lines_show(img1, img2, uvmat, F): # 右画像(二番目の画像)中の点に対応するエピポーラ線の計算 # 計算したエピポーラ線を左画像に描画 pts1 = np.array([[i[0], i[1]] for i in uvmat]) pts2 = np.array([[i[2], i[3]] for i in uvmat]) lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) img5, img6 = draw_lines(img1, img2, lines1, pts1, pts2) # 左画像(一番目の画像)中の点に対応するエピポーラ線の計算 # 計算したエピポーラ線を右画像に描画 lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = draw_lines(img2, img1, lines2, pts2, pts1) # 結果の表示 #plt.subplot(121), plt.imshow(img5) #plt.subplot(122), plt.imshow(img3) #plt.savefig("epipolar.jpg") plt.imshow(img5) plt.savefig("epipolar_img1.jpg") plt.imshow(img3) plt.savefig("epipolar_img2.jpg")
def draw_epilines(img1, img2, pt1, pt2, fmat): #convert the image to BGR form img1 = cvtColor(img1,COLOR_RGB2BGR) img2 = cvtColor(img2,COLOR_RGB2BGR) #get the size of the two images to calculate the end points weight1 = img1.shape[1] weight2 = img2.shape[1] #make the float into int pt1_x = np.int32(pt1[0]) pt1_y = np.int32(pt1[1]) p1 = (pt1_x,pt1_y) pt2_x = np.int32(pt2[0]) pt2_y = np.int32(pt2[1]) p2 = (pt2_x,pt2_y) #create point set to calculate the epilines pts_1 = [[p1[0],p1[1]],[0,0]] pts_1 = np.matrix(pts_1) pts_2 = [[p2[0],p2[1]],[0,0]] pts_2 = np.matrix(pts_2) #calculate epilines lines_2 = computeCorrespondEpilines(pts_1.reshape(-1,1,2), 2,fmat) lines_1 = computeCorrespondEpilines(pts_2.reshape(-1,1,2), 1,fmat) #draw2 #start point in draw 2 x_start_2 = 0 y_start_2 = -lines_2[0][0,2] /lines_2[0][0,1] pt_start_2 = (x_start_2, y_start_2) #end point in draw 2 x_end_2 = weight2 y_end_2 = np.int32((-lines_2[0][0,2] - lines_2[0][0,0] * x_end_2) /lines_2[0][0,1]) pt_end_2 = (x_end_2, y_end_2) image_2 = circle(img2, p2 , 20, (0, 255, 0), -1) image_2 = line(img2, pt_start_2, pt_end_2, (0, 255, 0), 3) draw2 = image_2 #draw1 #start point in draw 1 x_start_1 = 0 y_start_1 = -lines_1[0][0,2] /lines_1[0][0,1] pt_start_1 = (x_start_1, y_start_1) #end point in draw 1 x_end_1 = weight1 y_end_1 = np.int32((-lines_1[0][0,2] - lines_1[0][0,0] * x_end_1) /lines_1[0][0,1]) pt_end_1 = (x_end_1, y_end_1) image_1 = circle(img1, p1 , 20, (0, 255, 0), -1) image_1 = line(img1, pt_start_1, pt_end_1, (0, 255, 0), 3) draw1 = image_1 return draw1, draw2
def draw_epilines(img1, img2, pts1, pts2, F12): # get epilines in the img1 w.r.t points in the img2 lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F12) lines1 = lines1.reshape(-1, 3) # get epilines in the img2 w.r.t points in the img1 lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F12) lines2 = lines2.reshape(-1, 3) h1, w1 = img1.shape[0], img1.shape[1] h2, w2 = img2.shape[0], img2.shape[1] num_pts = len(pts1) colors = np.random.randint(0, 255, (num_pts, 3)) for r1, r2, pt1, pt2, color in zip(lines1, lines2, pts1, pts2, colors): color = tuple(color.tolist()) # two end points of epiline in img1 x1_l, y1_l = map(int, [0, -r1[2] / r1[1]]) x1_r, y1_r = map(int, [w1, -(r1[2] + r1[0] * w1) / r1[1]]) # two end points of epiline in img2 x2_l, y2_l = map(int, [0, -r2[2] / r2[1]]) x2_r, y2_r = map(int, [w2, -(r2[2] + r2[0] * w2) / r2[1]]) # draw line img1 = cv2.line(img1, (x1_l, y1_l), (x1_r, y1_r), color, 1) img2 = cv2.line(img2, (x2_l, y2_l), (x2_r, y2_r), color, 1) # draw point img1 = cv2.circle(img1, tuple(pt1), 5, color, -1) img2 = cv2.circle(img2, tuple(pt2), 5, color, -1) return img1, img2
def drawEpilinesAndMatches(imgL, imgR, pts1, pts2, kp1, kp2, good): # 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) img3_L = imgL img3_R = imgR img3_L, img3_R = drawlines(img3_L,img3_R,lines1,pts1,pts2) fig = plt.figure() plt.subplot(121) plt.imshow(imgL), plt.title('Input L (no lines should be written on these variables?)') plt.subplot(122) plt.imshow(imgR), plt.title('Input R (no lines should be written on these variables?)') plt.show() # 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) img4_L = imgL img4_R = imgR img4_L,img4_R = drawlines(img4_L,img4_R,lines2,pts2,pts1) # cv2.drawMatchesKnn expects list of lists as matches. # http://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_feature2d/py_matcher/py_matcher.html imgDummy = np.zeros((1,1)) img5 = cv2.drawMatches(imgL,kp1,imgR,kp2,good[:10],imgDummy) return img3_L, img3_R, img4_L, img4_R, img5, lines1, lines2
def rectify(image_1, image_2): # Rectify the images to make epipolar lines parallel pts_1, pts_2, H1, H2, F = features(image_1, image_2) pts_1 = np.int32(pts_1) pts_2 = np.int32(pts_2) rec1 = cv2.warpPerspective(image_1, H1, (720, 480)) rec2 = cv2.warpPerspective(image_2, H2, (720, 480)) F_rec = np.dot(np.dot(np.linalg.inv(H2).T, F), np.linalg.inv(H1)) lines_1 = cv2.computeCorrespondEpilines(pts_1.reshape(-1, 1, 2), 1, F_rec).reshape(-1, 3) lines_2 = cv2.computeCorrespondEpilines(pts_2.reshape(-1, 1, 2), 2, F_rec).reshape(-1, 3) draw_epiline(rec1, lines_2) draw_epiline(rec2, lines_1) h_stack = np.hstack((rec1, rec2)) cv2.imshow('rectify', h_stack) return rec1, rec2
def display_epilines(img1, img2, x1, x2, F): # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image cv2.namedWindow('epipolar lines and matches at img1', cv2.WINDOW_NORMAL) cv2.namedWindow('epipolar lines and matches at img2', cv2.WINDOW_NORMAL) lines1 = cv2.computeCorrespondEpilines(x2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) img3, img4 = draw_lines(img1, img2, lines1, x1, x2) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(x1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img5, img6 = draw_lines(img2, img1, lines2, x2, x1) if opencv: cv2.imshow('epipolar lines and matches at img1', img3) cv2.imshow('epipolar lines and matches at img2', img5) # ASCII(q) = 113, ASCII(esc) = 27, ASCII(space) = 32 while True: key = cv2.waitKey(0) & 0xFF if key == 113 or key == 27: cv2.destroyWindow('epipolar lines and matches at img1') cv2.destroyWindow('epipolar lines and matches at img2') break else: plt.subplot(121), plt.imshow(img3) plt.subplot(122), plt.imshow(img5) plt.show()
def createLine(event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: print("Creating Line for " + param[0]) print(param[1]) if param[0] == "Frame1": imageIndex = 1 otherIndex = 2 else: imageIndex = 2 otherIndex = 1 color = (randrange(256), randrange(256) , randrange(256)) point = np.array([x,y]) line = [0,0,0] line = cv2.computeCorrespondEpilines(point.reshape(-1,1,2), imageIndex, param[1]) line = line[0][0] size = param[2].shape[1] x0, y0 = map(int, [0, -line[2]/line[1]]) x1, y1 = map(int, [size, -(line[2]+(line[0]*float(size)))/line[1]]) cv2.line(param[2], (x0, y0),(x1, y1),color, 2) point = np.array([x1,y1]) line = [0,0,0] line = cv2.computeCorrespondEpilines(point.reshape(-1,1,2), otherIndex, param[1]) line = line[0][0] x0, y0 = map(int, [0, -line[2]/line[1]]) x1, y1 = map(int, [size, -(line[2]+(line[0]*float(size)))/line[1]]) cv2.line(param[3], (x0, y0),(x1, y1),color, 2)
def show_epilines(image1, img_points1, image2, img_points2, fundamental_mat): # Obtenemos las epilineas de ambas imágenes epipolarline_img1 = cv2.computeCorrespondEpilines(img_points1, 1, fundamental_mat).reshape(-1,3) epipolarline_img2 = cv2.computeCorrespondEpilines(img_points2, 2, fundamental_mat).reshape(-1,3) # Dibujamos las líneas epipolares # Lineas epipolares de la primera imagen sobre la segunda epip1, epip2 = draw_epilines(image1, img_points1, image2, img_points2, epipolarline_img2) canvas1 = np.zeros((epip1.shape[0],epip1.shape[1]+epip2.shape[1], 3), dtype = np.uint8) fx.insert_img_into_other(img_src=epip2, img_dest=canvas1, pixel_left_top_row=0, pixel_left_top_col=0, substitute=True) fx.insert_img_into_other(img_src=epip1, img_dest=canvas1, pixel_left_top_row=0, pixel_left_top_col=epip1.shape[1], substitute=True) # Lineas epipolares de la segunda imagen sobre la primera epip3, epip4 = draw_epilines(image2, img_points2, image1, img_points1, epipolarline_img1) canvas2 = np.zeros((epip3.shape[0],epip3.shape[1]+epip4.shape[1], 3), dtype = np.uint8) fx.insert_img_into_other(img_src=epip3, img_dest=canvas2, pixel_left_top_row=0, pixel_left_top_col=0, substitute=True) fx.insert_img_into_other(img_src=epip4, img_dest=canvas2, pixel_left_top_row=0, pixel_left_top_col=epip1.shape[1], substitute=True) # Mostramos ambas imágenes fx.show_img(canvas1, 'Epilineas') fx.show_img(canvas2, 'Epilineas') return epipolarline_img1, epipolarline_img2
def compute_and_drawlines(img1, img2, pts1, pts2, F): lines1 = cv.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) print(pts1.shape) lines1 = lines1.reshape(-1, 3) print('Shape of lines: ', lines1.shape) lines2 = cv.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 2, F) lines2 = lines2.reshape(-1, 3) ''' 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(lines1,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) imgplot = plt.imshow(img1) #plt.show() imgplot = plt.imshow(img2) #plt.show()''' return (lines1, lines2)
def draw_epilines(img1, img2, pt1, pt2, fmat): #... left_line = computeCorrespondEpilines( np.array(pt1).reshape(-1, 1, 2), 2, fmat) left_line = left_line.reshape(3, -1) right_line = computeCorrespondEpilines( np.array(pt2).reshape(-1, 1, 2), 1, fmat) right_line = right_line.reshape(3, -1) r1, k1, r2, k2 = img1.shape[0], img1.shape[1], img2.shape[0], img2.shape[1] val1, val2 = [0, int(-1 * right_line[2] / right_line[1])] val3, val4 = [ k1, int(-(right_line[2] + right_line[0] * k1) / right_line[1]) ] fig1 = line(img1, (val1, val2), (val3, val4), color=(0, 0, 0), thickness=4) fig1 = circle(img1, (int(pt1[0]), int(pt1[1])), radius=10, color=(200, 50, 40), thickness=-5) val5, val6 = [0, int(-left_line[2] / left_line[1])] val7, val8 = [k2, int(-(left_line[2] + left_line[0] * k1) / left_line[1])] fig2 = line(img2, (val5, val6), (val7, val8), color=(0, 0, 0), thickness=4) fig2 = circle(img2, (int(pt2[0]), int(pt2[1])), radius=10, color=(200, 50, 40), thickness=-5) return fig1, fig2
def do_epilines(F, left_img, left_idx, left_pts, right_img, right_idx, right_pts, prefix='epilines', p=dflt_params): # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv.computeCorrespondEpilines(right_pts.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) print_message(f"Computed Epilines lines1:\n{lines1}", params=p) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv.computeCorrespondEpilines(left_pts.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) print_message(f"Computed Epilines lines2:\n{lines2}", params=p) epiline_images = [] left_lines, _ = cv_drawlines(left_img, right_img, lines1, left_pts, right_pts) epiline_images.append((left_lines, f"{prefix}_{left_idx}-{right_idx}.bmp")) right_lines, _ = cv_drawlines(right_img, left_img, lines2, right_pts, left_pts) epiline_images.append( (right_lines, f"{prefix}_{right_idx}-{left_idx}.bmp")) output = [cv_save(f, i, params=p) for i, f in epiline_images] return output
def knnmatchandfundamentalmatrix(image1, image2, keypoints_tsucuba_left, descriptors_tsucuba_left, keypoints_tsucuba_right, descriptors_tsucuba_right): matcher = cv2.BFMatcher() matches = matcher.knnMatch(descriptors_tsucuba_left, descriptors_tsucuba_right, k=2) good_match = [] good_match_new = [] good_match_random = [] pts1 = [] pts2 = [] new_pts1 = [] new_pts2 = [] for i, j in matches: if i.distance < 0.75 * j.distance: good_match.append([i]) good_match_new.append(i) pts2.append(keypoints_tsucuba_right[i.trainIdx].pt) pts1.append(keypoints_tsucuba_left[i.queryIdx].pt) knn_image = cv2.drawMatchesKnn(image1, keypoints_tsucuba_left, image2, keypoints_tsucuba_right, good_match, None) cv2.imwrite('task2_matches_knn.jpg', knn_image) pts1 = np.int32(pts1) pts2 = np.int32(pts2) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.RANSAC) pts1 = pts1[mask.ravel() == 1] pts2 = pts2[mask.ravel() == 1] print(F) for i in range(10): rand = random.randint(0, len(pts1) - 1) new_pts1.append(pts1[rand]) new_pts2.append(pts2[rand]) new_pts1 = np.int32(new_pts1) new_pts2 = np.int32(new_pts2) # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv2.computeCorrespondEpilines(new_pts2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) img5, img6 = drawlines(image1, image2, lines1, new_pts1, new_pts2) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(new_pts1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(image2, image1, lines2, new_pts2, new_pts1) cv2.imwrite('task2_epi_right.jpg', img3) cv2.imwrite('task2_epi_left.jpg', img5) stereo = cv2.StereoBM_create(numDisparities=96, blockSize=31) disparity = stereo.compute(image1, image2) cv2.imwrite('task2_disparity.jpg', disparity) return
def compute_fundamental_matrix(img1_points, img2_points, img1=[], img2=[]): ''' By default openCV uses RANSAC with an 8 point sample. F is the fundamental matrix. Mask is a column vector with as many rows as points in imgi_points, if mask[j] == 0, then img1_points[j] and img2_points[j] are no longer considered as true correspondences. ''' F, mask = cv.findFundamentalMat(img1_points, img2_points) # Getting rid of the outliers. img1_points = img1_points[mask.flatten() == 1] img2_points = img2_points[mask.flatten() == 1] # Computing epipolar lines in order to compute each epipole. lines_img1 = cv.computeCorrespondEpilines(img2_points.reshape(-1, 1, 2), 2, F) lines_img2 = cv.computeCorrespondEpilines(img1_points.reshape(-1, 1, 2), 1, F) # Computing the epipole in the first image. epipole1 = np.cross(lines_img1[0], lines_img1[1]).flatten() if img1 != []: draw_epilines(img1, lines_img1, name='epilines1.png') # Compting the epipole in the second image. epipole2 = np.cross(lines_img2[0], lines_img2[1]).flatten() if img2 != []: draw_epilines(img2, lines_img2, name='epilines2.png') return F, img1_points.T, img2_points.T, epipole1, epipole2
def __epipolarsAverageDistanceTest (self, F1, F2): logger.info ('\nepipolarsAverageDistanceTest\n') logger.debug ('F1: ' + str (F1) + '\nF2: ' + '\ntype of F1: ' + str (type (F1)) + '\ntype of F2: ' + str (type (F2))) testPoints = np.int32 ([(rand.randint (0, 1000), rand.randint (0, 1000)) for i in range (1000)]).reshape (-1,1,2) lines1 = cv2.computeCorrespondEpilines (testPoints, 1, F1).reshape (-1,3) lines2 = cv2.computeCorrespondEpilines (testPoints, 1, F2).reshape (-1,3) logger.debug ('testPoints: ' + str (testPoints[:10])) logger.debug ('lines1:' + str (lines1[:10]) + '\nlines2:' + str (lines2[:10])) points1 = [-line[2] / line[1] for line in lines1] points2 = [-line[2] / line[1] for line in lines2] logger.debug ('points1: ' + str (points1[:10])) logger.debug ('points2: ' + str (points2[:10])) distances = [abs (point1 - point2) for (point1, point2) in zip (points1, points2)] logger.info ('distances: ' + str (distances[:10])) average1 = np.mean (distances) average2 = np.mean ([abs (distance - average1) for distance in distances]) logger.debug ('average1: ' + str (average1)) logger.debug ('average2: ' + str (average2)) return (average1, average2) pass
def draw_epipolar_lines(im1, im2, F, points1, points2): """ We followed the official opencv tutorial: https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_calib3d/py_epipolar_geometry/py_epipolar_geometry.html Draws the epipolar lines for the best fundamental matrix F and the keypoints for `img1` and `img2`. :param im1: :param im2: :param F: :param points1: :param points2: :return: """ # transform to grayscale im1 = cv2.cvtColor(im1, cv2.COLOR_RGB2BGR) im2 = cv2.cvtColor(im2, cv2.COLOR_RGB2BGR) # cv2.circle needs int points and not float -> type conversion points1 = points1.astype(int) points2 = points2.astype(int) # get epilines for first image that correspond to second image and draw them on the left lines1 = cv2.computeCorrespondEpilines(points2.reshape(-1, 1, 2), 2, F) # from list of lists to list lines1 = lines1.reshape(-1, 3) img5, img6 = plot_epipolar_line(im1, im2, lines1, points1, points2) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(points1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = plot_epipolar_line(im2, im1, lines2, points2, points1) # plot the two images with the epipolar lines plt.subplot(121) plt.imshow(img5) plt.subplot(122) plt.imshow(img3) plt.show()
def check_calibration(image_points_left, image_points_right, left_camMat, right_camMat, leftDistC, rightDistC, F, object_point, img_count): sides = "left", "right" which_image = {sides[0]: 1, sides[1]: 2} undistorted, lines = {}, {} undistorted["left"] = cv2.undistortPoints( np.concatenate(image_points_left).reshape(-1, 1, 2), left_camMat, leftDistC, P=left_camMat) lines["left"] = cv2.computeCorrespondEpilines(undistorted["left"], which_image["left"], F) undistorted["right"] = cv2.undistortPoints( np.concatenate(image_points_right).reshape(-1, 1, 2), right_camMat, leftDistC, P=right_camMat) lines["right"] = cv2.computeCorrespondEpilines(undistorted["right"], which_image["right"], F) total_error = 0 this_side, other_side = sides for side in sides: for i in range(len(undistorted[side])): total_error += abs( undistorted[this_side][i][0][0] * lines[other_side][i][0][0] + undistorted[this_side][i][0][1] * lines[other_side][i][0][1] + lines[other_side][i][0][2]) other_side, this_side = sides total_points = img_count * len(object_point) return total_error / total_points
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 drawEpilines(): bf = cv2.BFMatcher() matches = bf.knnMatch(des1, des2, k=2) matches_random = np.random.permutation(matches)[:10] good_random = [] pts1_rand = [] pts2_rand = [] for m, n in matches_random: good_random.append(m) pts1_rand.append(kp1[m.queryIdx].pt) pts2_rand.append(kp2[m.trainIdx].pt) pts1_rand = np.int32(pts1_rand) pts2_rand = np.int32(pts2_rand) F1, mask1 = cv2.findFundamentalMat(pts1_rand, pts2_rand, cv2.FM_LMEDS) linesLeft = cv2.computeCorrespondEpilines(pts2_rand.reshape(-1, 1, 2), 2, F1) linesLeft = linesLeft.reshape(-1, 3) img5, img6 = drawlines(img1, img2, linesLeft, pts1_rand, pts2_rand) linesRight = cv2.computeCorrespondEpilines(pts1_rand.reshape(-1, 1, 2), 1, F1) linesRight = linesRight.reshape(-1, 3) img7, img8 = drawlines(img2, img1, linesRight, pts2_rand, pts1_rand) cv2.imwrite('task2_epi_left.jpg', img5) cv2.imwrite('task2_epi_right.jpg', img7)
def sift3dImageAlignment(img1, img2): _, kp1, des1 = sift_kp(img1) _, kp2, des2 = sift_kp(img2) goodMatch = get_good_match(des1, des2) pts1 = np.float32([kp1[m.queryIdx].pt for m in goodMatch]).reshape(-1, 1, 2) pts2 = np.float32([kp2[m.trainIdx].pt for m in goodMatch]).reshape(-1, 1, 2) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS) if mask != None: pts1 = pts1[mask.ravel() == 1] pts2 = pts2[mask.ravel() == 1] # 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) img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) 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) cv2.namedWindow('Resultx', cv2.WINDOW_NORMAL) cv2.imshow('Resultx', img3) cv2.namedWindow('Resulty', cv2.WINDOW_NORMAL) cv2.imshow('Resulty', img5)
def draw_ep(M, img1, img2): fp1 = M[:, 0:2] fp2 = M[:, 2:4] # normalize fp1 = fp1.T fp2 = fp2.T # 2차원의 좌표를 homogeneous 로 변경 fp1 = np.insert(fp1, 2, 1, axis=0) fp2 = np.insert(fp2, 2, 1, axis=0) F = compute_F_norm(M) # Draw a polar line in the left image for the points in the right image lines1 = cv2.computeCorrespondEpilines(fp2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) img5, img6 = drawlines(img1, img2, lines1, fp1, fp2) # In the left picture, draw the epipolar line in the right picture lines2 = cv2.computeCorrespondEpilines(fp1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(img2, img1, lines2, fp2, fp1) plt.subplot(121), plt.imshow(img5) plt.subplot(122), plt.imshow(img3) plt.show()
def disp_epipoles(self, img0, img1, F): img0 = img0.copy() img1 = img1.copy() def drawlines(img, lines, pts): r, c, _ = img.shape for r, pt in zip(lines, pts): 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]]) cv2.line(img, (x0, y0), (x1, y1), color, 1) cv2.circle(img, tuple(pt), 5, color, -1) return img epilines1 = cv2.computeCorrespondEpilines( self.sorted_keypts_1.reshape(-1, 1, 2), 2, F) epilines1 = epilines1.reshape(-1, 3) img0_ = drawlines(img0, epilines1, self.sorted_keypts_0) epilines2 = cv2.computeCorrespondEpilines( self.sorted_keypts_0.reshape(-1, 1, 2), 1, F) epilines2 = epilines2.reshape(-1, 3) img1_ = drawlines(img1, epilines2, self.sorted_keypts_1) cv2.imshow(f"img{self.img_count}", img0_) self.img_count += 1 cv2.imshow(f"img{self.img_count}", img1_) self.img_count += 1 cv2.waitKey(0)
def computeEpipole(self, clickLoc, I, d=[-100, 100], n=20): pt = np.array([[clickLoc.x()], [clickLoc.y()]]) if self.mode == 'openCV': if I == 'L': lineParams = cv2.computeCorrespondEpilines( pt, 1, self.calData['F']) elif I == 'R': lineParams = cv2.computeCorrespondEpilines( pt, 2, self.calData['F']) print(lineParams) else: pass h = self.normalizePixel( pt, self.calData['fc_left'], self.calData['cc_left'], self.calData['kc_left'], self.calData['alpha_c_left']) uo = np.hstack((h[0], h[1], 1)) T = self.calData['T'] R = self.calData['R'] S = np.array([[0, -T[2], T[1]], [T[2], 0, -T[0]], [-T[1], T[0], 0]]) l_epipole = np.dot(np.dot(S, R), uo.transpose()) KK_right = np.array([ [self.calData['fc_right'][0][0], self.calData['alpha_c_right'][0][0] * self.calData['fc_right'][0][0], self.calData['cc_right'][0][0]], [0, self.calData['fc_right'][1][0], self.calData['cc_right'][1][0]], [0, 0, 1]]) if np.sqrt(np.dot(l_epipole[1], l_epipole[1])) > np.sqrt(np.dot(l_epipole[0], l_epipole[0])): limit_x_pos = ( (pt[1][0] + d[1]) - self.calData['cc_right'][0]) / self.calData['fc_right'][0] limit_x_neg = ( (pt[1][0] - d[0]) - self.calData['cc_right'][0]) / self.calData['fc_right'][0] x_list = (limit_x_pos - limit_x_neg) * (np.array(range(n)).astype('float') / (float(n) - 1)) + limit_x_neg pt = np.cross(np.tile(np.array([l_epipole]).T, (1, n)).T, np.array( [np.ones((1, n))[0], np.zeros((1, n))[0], -x_list]).T).T else: limit_y_pos = ( (pt[1][0] + d[1]) - self.calData['cc_right'][1]) / self.calData['fc_right'][1] limit_y_neg = ( (pt[1][0] - d[0]) - self.calData['cc_right'][1]) / self.calData['fc_right'][1] y_list = (limit_y_pos - limit_y_neg) * \ (range(n) / (n - 1)) + limit_y_neg pt = np.cross(np.tile(l_epipole, (1, n)), np.array( [[np.zeros((1, n))], [np.ones(1, n)], [-y_list]])) pt = np.vstack((pt[0, :] / pt[2, :], pt[1, :] / pt[2, :])) ptd = self.applyDistortion(pt, self.calData['kc_right']) epipole = np.dot(KK_right, np.vstack((ptd, np.ones((1, n))))) return epipole[0:2, :]
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 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 calculate_cost(cam1, person1, cam2, person2): """ calculate the epipolar distance between two humans :param cam1: :param person1: :param cam2: :param person2: :return: """ F = get_fundamental_matrix(cam1.P, cam2.P) J = len(person1) assert J == len(person2) # drop all points that are -1 -1 (not visible) pts1 = [] pts2 = [] weights1 = [] weights2 = [] for jid in range(J): x1, y1, w1 = person1[jid] x2, y2, w2 = person2[jid] if x1 >= 0 and x2 >= 0: pts1.append((x1, y1)) weights1.append(w1) pts2.append((x2, y2)) weights2.append(w2) weights1 = np.clip(weights1, a_min=0, a_max=1) weights2 = np.clip(weights2, a_min=0, a_max=1) if len(pts1) == 0: return np.finfo(np.float32).max pts1 = np.array(pts1) pts2 = np.array(pts2) epilines_1to2 = np.squeeze( cv2.computeCorrespondEpilines(pts1, 1, F)) epilines_2to1 = np.squeeze( cv2.computeCorrespondEpilines(pts2, 2, F)) total = 0 n_pairs = len(pts1) assert n_pairs == len(pts2) if n_pairs == 1: epilines_1to2 = np.expand_dims(epilines_1to2, axis=0) epilines_2to1 = np.expand_dims(epilines_2to1, axis=0) for p1, l1to2, w1, p2, l2to1, w2 in zip( pts1, epilines_1to2, weights1, pts2, epilines_2to1, weights2): d1 = gm.line_to_point_distance(*l1to2, *p2) d2 = gm.line_to_point_distance(*l2to1, *p1) total += d1 + d2 return total / n_pairs # normalize
def draw_epilines(img1, kp1, img2, kp2, matches, fund): ''' Draws epilines between two images. Args: ... ''' _, cols, _ = img1.shape # Place images side by side img = combine_images(img1, img2) colors = random_colors() pts1 = np.float32( [kp1[m.queryIdx].pt for m in matches] ).reshape(-1, 1, 2) pts2 = np.float32( [kp2[m.trainIdx].pt for m in matches] ).reshape(-1, 1, 2) lines2 = cv2.computeCorrespondEpilines(pts1, 1, fund) lines1 = cv2.computeCorrespondEpilines(pts2, 2, fund) try: # Draw matches for i in range(0, len(pts1)): color = colors[i%len(colors)] x1, y1 = int(pts1[i][0][0]), int(pts1[i][0][1]) x2, y2 = int(pts2[i][0][0]), int(pts2[i][0][1]) x2 += cols a1, b1, c1 = lines1[i][0] a2, b2, c2 = lines2[i][0] cv2.circle(img, (x1,y1), 8, color, 2) cv2.circle(img, (x2,y2), 8, color, 2) y11 = -c1 / b1 # At x = 0 y12 = (-a1*cols-c1) / b1 # At x = col y21 = -c2 / b2 # At x = 0 y22 = (-a2*cols-c2) / b2 # At x = col cv2.line(img, (0, int(y11)), (cols, int(y12)), color, 1) cv2.line(img, (cols, int(y21)), (2*cols, int(y22)), color, 1) return img except: print 'Warning: Could not create epilines' return None
def shi_tomasi(gray): # image:源图片 # maxCorners:最佳角点数量 # qualityLevel:质量等级,小于此质量等级的角点都会被忽略 # minDistance:交点之间的最小距离 corners = cv2.goodFeaturesToTrack(gray, 25, 0.01, 10) cv2.computeCorrespondEpilines() # 返回的结果是 [[ 311., 250.]] 两层括号的数组。 corners = np.int0(corners) return corners
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 drawEpipolarLines(Xp_1,Xp_2,F,im_1,im_2): F = np.mat(F) #get epipolar lines lines1 = cv2.computeCorrespondEpilines(Xp_2.reshape(-1,1,2), 2,F) lines1 = lines1.reshape(-1,3) img3,img4 = clsReconstruction.drawlines(im_1,im_2,lines1,Xp_1,Xp_2) lines2 = cv2.computeCorrespondEpilines(Xp_1.reshape(-1,1,2), 1,F) lines2 = lines2.reshape(-1,3) img5, img6 = clsReconstruction.drawlines(im_2, im_1, lines2,Xp_2, Xp_1) plt.subplot(121),plt.imshow(img3) plt.subplot(122),plt.imshow(img5) plt.show()
def check_calibration(self, calibration): """ Check calibration quality by computing average reprojection error. First, undistort detected points and compute epilines for each side. Then compute the error between the computed epipolar lines and the position of the points detected on the other side for each point and return the average error. """ sides = "left", "right" which_image = {sides[0]: 1, sides[1]: 2} undistorted, lines = {}, {} for side in sides: undistorted[side] = cv2.undistortPoints( np.concatenate(self.image_points[side]).reshape(-1, 1, 2), calibration.cam_mats[side], calibration.dist_coefs[side], P=calibration.cam_mats[side]) lines[side] = cv2.computeCorrespondEpilines(undistorted[side], which_image[side], calibration.f_mat) total_error = 0 this_side, other_side = sides for side in sides: for i in range(len(undistorted[side])): total_error += abs(undistorted[this_side][i][0][0] * lines[other_side][i][0][0] + undistorted[this_side][i][0][1] * lines[other_side][i][0][1] + lines[other_side][i][0][2]) other_side, this_side = sides total_points = self.image_count * len(self.object_points) return total_error / total_points
def print_epilines(self, Fcv, i_next, i_prev, points1, points2): lines1 = cv2.computeCorrespondEpilines(np.float32(points2).reshape(-1, 1, 2), 2, Fcv) lines1 = lines1.reshape(-1, 3) img5, img6 = drawlines(self.images[i_prev], self.images[i_next], lines1, np.int32(points1), np.int32(points2)) plt.subplot(121), plt.imshow(img5) plt.subplot(122), plt.imshow(img6) plt.show()
def draw_epilines(img1, img2, points_img1, points_img2, fundamental_matrix): # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv2.computeCorrespondEpilines(points_img2.reshape(-1, 1, 2), 2, fundamental_matrix) lines1 = lines1.reshape(-1, 3) img5, img6 = drawlines(img1, img2, lines1, points_img1, points_img2) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(points_img1.reshape(-1, 1, 2), 1, fundamental_matrix) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(img2, img1, lines2, points_img2, points_img1) plt.figure(figsize=(20, 20)) plt.subplot(211), plt.imshow(img5) plt.subplot(212), plt.imshow(img3) plt.show() return lines1, lines2
def draw_epilines(src_pts, dst_pts, img1, img2, F, mask): # select only inlier points img1_pts = src_pts[mask.ravel()==1] img2_pts = dst_pts[mask.ravel()==1] # find epilines corresponding to points in the 2nd image and draw them on the 1st image lines1 = cv2.computeCorrespondEpilines(img2_pts.reshape(-1,1,2), 2, F) lines1 = lines1.reshape(-1,3) img5, img6 = draw_lines(img1, img2, lines1, img1_pts, img2_pts) # find epilines corresponding to points in the 1st image and draw them on the 2nd image lines2 = cv2.computeCorrespondEpilines(img1_pts.reshape(-1,1,2), 1, F) lines2 = lines2.reshape(-1,3) img3, img4 = draw_lines(img2, img1, lines2, img2_pts, img1_pts) plt.subplot(121), plt.imshow(img5) plt.subplot(122), plt.imshow(img3) plt.show()
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 match(img1, img2, K, distort): #plotter.plot2(img1) timeStart = time.time() img1 = cv2.undistort(img1,K,distort) img2 = cv2.undistort(img2,K,distort) pts1, pts2, des1, des2 = correspondences.getCorrespondences(img1,img2) print("Time for correspondences: "+str(time.time()-timeStart)) if(len(pts1)<8): print("ERROR: <8 correspondeces") return timeStart = time.time() F, mask = computervision.findFundamentalMatrix(K,pts1,pts2) print("Time for Fundamental: "+str(time.time()-timeStart)) #F = F/np.linalg.norm(F) pts1 = pts1[mask.ravel()==1] pts2 = pts2[mask.ravel()==1] des1 = [des1[ind] for ind, x in enumerate(mask) if x==1] des2 = [des2[ind] for ind, x in enumerate(mask) if x==1] timeStart = time.time() if(pts1.shape[0]>8): F = computervision.nonlinearOptimizationFundamental(F,K,pts1,pts2) print("Time for nonlinearOptimizationFundamental: "+str(time.time()-timeStart)) testFundamentalMatrix(F,pts1,pts2) lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F) lines1 = lines1.reshape(-1,3) img5 = drawlines(img1,img2,lines1,pts1,pts2,K) lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F) lines2 = lines2.reshape(-1,3) img3 = drawlines(img2,img1,lines2,pts2,pts1,K) p1, p2, X, rot, trans = computervision.getCameraMatrix(F,K,pts1,pts2) print("Translation: "+str(trans)) plotter.plot(rot,trans,X,img1,pts1) reprojectionError(X,pts1,pts2,p1,p2) cubePosition = X[0]#np.array([0,0,50,1])# projectPoint(img5,X,p1) projectPoint(img3,X,p2) projectCube(img5,p1,cubePosition) projectCube(img3,p2,cubePosition) vis = showCombinedImgs(img5,img3) drawCorrespondences(vis,pts1,pts2) cv2.imshow("test", vis) return patch.makePatches(pts1,pts2,X,des1,des2)
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 drawEpiline(img1, img2, pts1, pts2, F): pts1,pts2 = pts1[:30,:], pts2[:30,:] #scale down a little bit # 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 visualize(I1, I2, F, pt1, pt2): # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image #lines1 = cv2.computeCorrespondEpilines(pt2.reshape(-1, 1, 2), 1, F) #lines1 = lines1.reshape(-1, 3) #img1, img2 = drawlines(I1, I2, lines1, pt1, pt2) #cv2.imshow("Like In Assignment PDF", img1) #cv2.imshow("Like In Assignment PDF_2", img2) #cv2.waitKey(0) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(pt1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) img3, img4 = drawlines(I2, I1, lines2, pt2, pt1) cv2.imshow("Like In Assignment PDF", img3) cv2.imshow("Like In Assignment PDF_2", img4) cv2.waitKey(0)
def find_fund_mat(src_pts, dst_pts, threshold=1.5): n_pts = src_pts.shape[0] src_pts_ext = np.concatenate((src_pts, np.ones((src_pts.shape[0], 1))), axis=1) dst_pts_ext = np.concatenate((dst_pts, np.ones((dst_pts.shape[0], 1))), axis=1) F = np.zeros((3, 3)) matchesMask = np.zeros(n_pts) max_vote = -1 population = range(n_pts) for k in range(2000): eight_point_filter = tuple(sorted(random.sample(population, 8))) temp_F = find_fund_mat_eight_pts(src_pts_ext[eight_point_filter, :], dst_pts_ext[eight_point_filter, :]) if temp_F is not None: epilines = cv2.computeCorrespondEpilines(src_pts.reshape(-1, 1, 2), 1, temp_F).reshape(-1, 3) dists = np.abs(np.sum(epilines * dst_pts_ext, axis=1)) temp_mask = dists <= threshold if np.sum(temp_mask) > max_vote: max_vote = np.sum(temp_mask) F = temp_F matchesMask = temp_mask return F, matchesMask
def compute_F(im1, im2): if show_rt: t = time.time() #get points in frame A #get sift points to match (kp1) kp1, des1 = sift.detectAndCompute(im1, None) #get points in frame B #get sift points to match (kp2) kp2, des2 = sift.detectAndCompute(im2, None) #only take points that are in both images to compute fundamental matrix # 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) #cleaner, from http://docs.opencv.org/master/da/de9/tutorial_py_epipolar_geometry.html#gsc.tab=0 #no idea whats going on here good = [] pts1 = [] pts2 = [] # ratio test as per Lowe's paper for i,(m,n) in enumerate(matches): if m.distance < 0.6*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) pts1 = pts1.astype(float) pts2 = pts2.astype(float) #compute fundamental matrix based on given keypoints (F) F, mask = cv2.findFundamentalMat(pts1.astype(float),pts2.astype(float),cv2.FM_RANSAC) #F, mask = cv2.findFundamentalMat(pts1, pts2, method=CV_FM_RANSAC) ^^cv2.FM_LMEDS pts1 = pts1[mask.ravel() == 1] pts2 = pts2[mask.ravel() == 1] pts1 = np.float32(pts1) pts2 = np.float32(pts2) if UNIT_TESTING: # We select only inlier points # 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(im1,im2,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(im2,im1,lines2,pts2,pts1) #print '\n', 'img5:', '\n', img5, '\n' #cv2.imshow('img5', img5) #cv2.imshow('img3', img3) #cv2.waitKey(0) plt.subplot(121),plt.imshow(img5) plt.subplot(122),plt.imshow(img3) plt.show() if DEBUGGING: print '\nFundamental Matrix:\n', F, '\n' if show_rt: t = time.time() - t print '\ncompute_F() runtime:', t, 'secs\n' return F, pts1, pts2
def StereoCameraCalibration() : # Calibrate the left camera cam1 = CameraCalibration( sorted( glob.glob( '{}/left*.png'.format(calibration_directory) ) ) ) # Calibrate the right camera cam2 = CameraCalibration( sorted( glob.glob( '{}/right*.png'.format(calibration_directory) ) ) ) # Stereo calibration termination criteria criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5) # Stereo calibration flags flags = 0 flags |= cv2.CALIB_USE_INTRINSIC_GUESS # flags |= cv2.CALIB_FIX_INTRINSIC # flags |= cv2.CALIB_FIX_PRINCIPAL_POINT # flags |= cv2.CALIB_FIX_FOCAL_LENGTH flags |= cv2.CALIB_FIX_ASPECT_RATIO # flags |= cv2.CALIB_SAME_FOCAL_LENGTH # flags |= cv2.CALIB_ZERO_TANGENT_DIST flags |= cv2.CALIB_RATIONAL_MODEL flags |= cv2.CALIB_FIX_K3 flags |= cv2.CALIB_FIX_K4 flags |= cv2.CALIB_FIX_K5 # Stereo calibration calibration = cv2.stereoCalibrate( cam1['obj_points'], cam1['img_points'], cam2['img_points'], cam1['camera_matrix'], cam1['dist_coefs'], cam2['camera_matrix'], cam2['dist_coefs'], cam1['img_size'], flags=flags, criteria=criteria ) # Store the stereo calibration results in a dictionary parameter_names = ( 'calib_error', 'camera_matrix_l', 'dist_coefs_l', 'camera_matrix_r', 'dist_coefs_r', 'R', 'T', 'E', 'F' ) calibration = dict( zip( parameter_names, calibration ) ) # Stereo rectification rectification = cv2.stereoRectify( calibration['camera_matrix_l'], calibration['dist_coefs_l'], calibration['camera_matrix_r'], calibration['dist_coefs_r'], cam1['img_size'], calibration['R'], calibration['T'], flags=0 ) # Store the stereo rectification results in the dictionary parameter_names = ( 'R1', 'R2', 'P1', 'P2', 'Q', 'ROI1', 'ROI2' ) calibration.update( zip( parameter_names, rectification ) ) # Undistortion maps calibration['left_map'] = cv2.initUndistortRectifyMap( calibration['camera_matrix_l'], calibration['dist_coefs_l'], calibration['R1'], calibration['P1'], cam1['img_size'], cv2.CV_32FC1 ) calibration['right_map'] = cv2.initUndistortRectifyMap( calibration['camera_matrix_r'], calibration['dist_coefs_r'], calibration['R2'], calibration['P2'], cam2['img_size'], cv2.CV_32FC1 ) # Compute reprojection error undistorted_l = cv2.undistortPoints( np.concatenate( cam1['img_points'] ).reshape(-1, 1, 2), calibration['camera_matrix_l'], calibration['dist_coefs_l'], P=calibration['camera_matrix_l'] ) undistorted_r = cv2.undistortPoints( np.concatenate( cam2['img_points'] ).reshape(-1, 1, 2), calibration['camera_matrix_r'], calibration['dist_coefs_r'], P=calibration['camera_matrix_r'] ) lines_l = cv2.computeCorrespondEpilines( undistorted_l, 1, calibration['F'] ) lines_r = cv2.computeCorrespondEpilines( undistorted_r, 2, calibration['F'] ) calibration['reproject_error'] = 0 for i in range( len( undistorted_l ) ) : calibration['reproject_error'] += abs( undistorted_l[i][0][0] * lines_r[i][0][0] + undistorted_l[i][0][1] * lines_r[i][0][1] + lines_r[i][0][2] ) + abs( undistorted_r[i][0][0] * lines_l[i][0][0] + undistorted_r[i][0][1] * lines_l[i][0][1] + lines_l[i][0][2] ) calibration['reproject_error'] /= len( undistorted_r ) # Write calibration results with open( '{}/calibration.log'.format(calibration_directory) , 'w') as output_file : output_file.write( '\n~~~ Left camera calibration ~~~\n\n' ) output_file.write( 'Calibration error : {}\n'.format( cam1['calib_error'] ) ) output_file.write( 'Reprojection error : {}\n'.format( cam1['reproject_error'] ) ) output_file.write( 'Camera matrix :\n{}\n'.format( cam1['camera_matrix'] ) ) output_file.write( 'Distortion coefficients :\n{}\n'.format( cam1['dist_coefs'].ravel() ) ) output_file.write( '\n~~~ Right camera calibration ~~~\n\n' ) output_file.write( 'Calibration error : {}\n'.format( cam2['calib_error'] ) ) output_file.write( 'Reprojection error : {}\n'.format( cam2['reproject_error'] ) ) output_file.write( 'Camera matrix :\n{}\n'.format( cam2['camera_matrix'] ) ) output_file.write( 'Distortion coefficients :\n{}\n'.format( cam2['dist_coefs'].ravel() ) ) output_file.write( '\n~~~ Stereo camera calibration ~~~\n\n' ) output_file.write( 'Stereo calibration error : {}\n'.format( calibration['calib_error'] ) ) output_file.write( 'Reprojection error : {}\n'.format( calibration['reproject_error'] ) ) output_file.write( 'Left camera matrix :\n{}\n'.format( calibration['camera_matrix_l'] ) ) output_file.write( 'Left distortion coefficients :\n{}\n'.format( calibration['dist_coefs_l'].ravel() ) ) output_file.write( 'Right camera matrix :\n{}\n'.format( calibration['camera_matrix_r'] ) ) output_file.write( 'Right distortion coefficients :\n{}\n'.format( calibration['dist_coefs_r'].ravel() ) ) output_file.write( 'Rotation matrix :\n{}\n'.format( calibration['R'] ) ) output_file.write( 'Translation vector :\n{}\n'.format( calibration['T'].ravel() ) ) output_file.write( 'Essential matrix :\n{}\n'.format( calibration['E'] ) ) output_file.write( 'Fundamental matrix :\n{}\n'.format( calibration['F'] ) ) output_file.write( 'Rotation matrix for the first camera :\n{}\n'.format( calibration['R1'] ) ) output_file.write( 'Rotation matrix for the second camera :\n{}\n'.format( calibration['R2'] ) ) output_file.write( 'Projection matrix for the first camera :\n{}\n'.format( calibration['P1'] ) ) output_file.write( 'Projection matrix for the second camera :\n{}\n'.format( calibration['P2'] ) ) output_file.write( 'Disparity-to-depth mapping matrix :\n{}\n'.format( calibration['Q'] ) ) output_file.write( 'ROI for the left camera : {}\n'.format( calibration['ROI1'] ) ) output_file.write( 'ROI for the right camera : {}\n'.format( calibration['ROI2'] ) ) # Write the calibration object with all the parameters with open( '{}/calibration.pkl'.format(calibration_directory) , 'wb') as output_file : pickle.dump( calibration, output_file, pickle.HIGHEST_PROTOCOL ) # Return the calibration return calibration
def stereoCalibrate(): board_n = board_w * board_h board_sz = (board_w, board_h) # cv2.namedWindow('Stereo Calibration', cv2.WINDOW_AUTOSIZE) board_pts = inverted_seq(board_sz, square_sz) image1, image2, image1_pts, image2_pts, object_pts, imgSize = getMatchedPtsByChessboard( [3], board_sz, board_pts) #image1, image2, image1_pts, image2_pts, object_pts, imgSize = getMatchedPts() # print image1_pts, image2_pts, object_pts, imgSize cameraMatrix1 = np.identity(3, np.float32); cameraMatrix2 = np.identity(3, np.float32) distCoeffs1 = np.zeros((5), np.float32); distCoeffs2 = np.zeros((5), np.float32) #CALIBRATE THE CAMERA flags = cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_SAME_FOCAL_LENGTH criteria = (1 + 2, 100, 1e-5) # 1 for MAX_ITER and 2 for EPS |= =| retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate( object_pts, image1_pts, image2_pts, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imgSize, flags = flags, criteria = criteria) print 'Re-projection error', retval print 'Intrinsic matrix 1\n', cameraMatrix1 print 'Intrinsic matrix 2\n', cameraMatrix2 print 'Distortion coeffs 1\n', distCoeffs1 print 'Distortion coeffs 2\n', distCoeffs2 print 'Rotation matrix\n', R print 'Translation vector\n', T print 'Eigen matrix\n', E print 'Fundamental matrix\n', F cameraMatrix = cameraMatrix1 cameraMatrix[:2,2] = (imgSize[0] / 2, imgSize[1] / 2) distCoeffs = np.zeros((1, 5), np.float32) if True: distCoeffs1 = distCoeffs2 = distCoeffs cameraMatrix1 = cameraMatrix2 = cameraMatrix image1 = cv2.undistort(image1, cameraMatrix1, distCoeffs1) image2 = cv2.undistort(image2, cameraMatrix2, distCoeffs2) #CALIBRATION QUALITY CHECK #Using the epipolar geometry constraint: m2' * F * m1 = 0 assert image1_pts[0].shape == image2_pts[0].shape newshape = image1_pts[0].shape newshape = (newshape[0], 1, newshape[1]) #Work in undistorted space (An undocumented signature is used here. @Dstray) image1_pts = cv2.undistortPoints(image1_pts[0].reshape(newshape, order = 'F'), cameraMatrix1, distCoeffs1, P = cameraMatrix1) image2_pts = cv2.undistortPoints(image2_pts[0].reshape(newshape, order = 'F'), cameraMatrix2, distCoeffs2, P = cameraMatrix2) points1, points2 = image1_pts.reshape(-1, 2), image2_pts.reshape(-1, 2) #implemented in opencv 3.0.0 if False: lines1 = cv2.computeCorrespondEpilines(points2, 2, F).reshape(-1, 3) lines2 = cv2.computeCorrespondEpilines(points1, 1, F).reshape(-1, 3) image1, image2 = drawlines(image1, image2, lines1, points2) image2, image1 = drawlines(image2, image1, lines2, points1) cv2.imshow('Image 1', image1); cv2.imshow('Image 2', image2) cv2.waitKey(0) #COMPUTE AND DISPLAY RECTIFICATION urmaps, Q = bouguetRectify(image1_pts, image2_pts, imgSize, cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2, R, T) '''urmaps, F = hartleyRectify(image1_pts, image2_pts, imgSize, cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2, F)''' print 'Q\n', Q Q = np.float32([[1, 0, 0, -0.5*imgSize[0]], [0, 1, 0, -0.5*imgSize[1]], [0, 0, 0, cameraMatrix[0][0]], [0, 0, 0.07, 0]]) #Save the results np.save('rectmap.npy', urmaps) np.save('Q.npy', Q) #RECTIFY THE IMAGES AND FIND DISPARITY MAPS #Setup for finding correspondences img1r = cv2.remap(image1, urmaps[0], urmaps[1], cv2.INTER_LINEAR) img2r = cv2.remap(image2, urmaps[2], urmaps[3], cv2.INTER_LINEAR) cv2.imshow('Image 1', img1r); cv2.imshow('Image 2', img2r) cv2.waitKey(0) '''disp, points = stereoBMMatch(img1r, img2r, Q) colors = cv2.cvtColor(image1, cv2.COLOR_BGR2RGB) mask = disp > disp.min() points = points[mask]; colors = colors[mask] write_ply('_3DPoints.ply', points, colors)''' #stereosgbm_match(img1r, img2r, '_3DPoints.ply', Q) cv2.waitKey(0) cv2.destroyAllWindows()
def main(): # Now ask for input print('\nThis program takes an image of a room and lets you know which room you are most likely in.\n') print('It then computes how closely your image matches an image in the database') print('If it is a close enough match-- it will tell you your current camera position in relatoin to teh camera position for teh photo it matched with.\n') image = raw_input("Please enter the path of the image containing a room: ") #get current working directory mypath = os.getcwd() #append /rooms mypath = mypath + '/Rooms' #get array of directory paths aka rooms directories = [x[0] for x in os.walk(mypath)] #remove rooms directory del directories[0] rooms = len(directories) #set up array for greatest match for i in range(rooms): roomMatched.insert(i, 1000000) photoMatched.insert(i, 100) #get images in each room and put into images array for directory in range(len(directories)): files = [x[2] for x in os.walk(directories[directory])] images.insert(directory, files) #blur original image blurredOriginal = blurImages(image) # Initiate ORB detector orb = cv2.ORB_create() #determine keypoints and their descriptors from teh current image kp1, des1 = orb.detectAndCompute(blurredOriginal,None) #Loop through each image in database for i in range(len(directories)): for j in range(len(images[i][0])): #get database image img2location = directories[i] + '/' + images[i][0][j] img2 = blurImages(img2location) # find the keypoints and descriptors with ORB kp2, des2 = orb.detectAndCompute(img2, None) #kp2, des2 = orb.detectAndCompute(img2,None) bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) # Match descriptors. matches = bf.match(des1,des2) # Sort them in the order of their distance. matches = sorted(matches, key = lambda x:x.distance) #get average distance away from 10 best matching points dist = 0 diffSize = [] diffAngle = [] for x in range(10): dist += matches[x].distance #put ten angle differences into diffAngle[] ang = kp1[matches[x].queryIdx].angle ang2 = kp2[matches[x].trainIdx].angle angDif = ang - ang2 diffAngle.insert(x, angDif) #put ten size differences into diffSize[] size1 = kp1[matches[x].queryIdx].size size2 = kp2[matches[x].trainIdx].size sizeDiff = size1 - size2 diffSize.insert(x, sizeDiff) dist = dist/10 stdDevAng = np.std(diffAngle)/10 stdDevSize = np.std(diffSize)/10 #alter dist to be the dist*stdDev*stdDev dist = dist*stdDevSize*stdDevAng #update least distance if likely if roomMatched[i] > dist: roomMatched[i] = dist photoMatched[i] = j #get the index of the most likely room by lowest distance mostLikely = roomMatched.index(min(roomMatched)) print('\n\n******OUTPUT*********') if (roomMatched[mostLikely] > 30): print('You are most likely in room ', mostLikely + 1, ' but we cannot say this with confidence as the current photos score is: ', roomMatched[mostLikely], ', which is above our cutoff point of 30.') else: closestImageLocation = directories[mostLikely] + '/' + images[mostLikely][0][photoMatched[mostLikely]] print('You are most likely in room: ', mostLikely + 1) print('Closest Image: ', closestImageLocation) print('Distance: ', roomMatched[mostLikely], ' (the closer to zero the higher the probability you are in given room)') #intrinsic parameters of camera -- found using matlab K = np.float32([3514.89135426281, 0, 1514.08070548149, 0, 3528.06290049699, 2019.41405283900, 0, 0, 1]).reshape(3,3) K_inv = np.linalg.inv(K) img2 = cv2.imread(closestImageLocation, cv2.IMREAD_GRAYSCALE) ###Plot best image with test image # find the keypoints and descriptors with ORB kp2, des2 = orb.detectAndCompute(img2,None) bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) # Match descriptors. matches = bf.match(des1,des2) # Sort them in the order of their distance. matches = sorted(matches, key = lambda x:x.distance) srcPts = [] dstPts = [] # get best 40 points from matched image and source image for f in range(40): srcPt = kp1[matches[f].queryIdx].pt dstPt = kp2[matches[f].trainIdx].pt srcPts.insert(f, srcPt) dstPts.insert(f, dstPt) #Fill arrays with 40 matching points src_pts = np.float32([srcPts[m] for m in range(len(srcPts))]) dst_pts = np.float32([dstPts[m] for m in range(len(dstPts))]) #Compute the fundamental matrix using ransac F, mask = cv2.findFundamentalMat(src_pts,dst_pts,cv2.FM_RANSAC, 1) # Selecting only the inliers pts1 = src_pts[mask.ravel()==1] pts2 = dst_pts[mask.ravel()==1] F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_RANSAC, 1) pts1 = pts1[mask.ravel() == 1] pts2 = pts2[mask.ravel() == 1] print("Fundamental matrix: \n", F) # Find Error in matrix using x'^t(F)x = 0 pt1 = np.array([[pts1[0][0]], [pts1[0][1]], [1]]) pt2 = np.array([[pts2[0][0], pts2[0][1], 1]]) fundMatrixError = (pt2.dot(F)).dot(pt1) #based off one point match print ("Error in fundamental matrix: ", fundMatrixError) #Draw epilines and matching points on images lines = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F) lines = lines.reshape(-1,3) lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F) lines2 = lines2.reshape(-1,3) BMEpilines = blurredOriginal.copy() CurrEpilines = img2.copy() BMEpilines = drawEpilines(BMEpilines,lines,pts1,pts2) CurrEpilines = drawEpilines(CurrEpilines,lines2,pts2,pts1) #Calculate Essential Matrix E = K.transpose().dot(F).dot(K) #Decomposing matrices of E U, diag110, Vt = np.linalg.svd(E) if np.linalg.det(np.dot(U,Vt))<0: Vt = -Vt E = np.dot(U,np.dot(np.diag([1.0,1.0,0.0]),Vt)) U, diag110, Vt = np.linalg.svd(E) print("Essential matrix: \n", E) #W is a rotation matrix that W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3) #normalize/ homogenize image coords by applying the camera instrinic params to each point for i in range(len(pts1)): normalizedSourcePoints.append(K_inv.dot([pts1[i][0], pts1[i][1], 1.0])) normalizedCurrentPoints.append(K_inv.dot([pts2[i][0], pts2[i][1], 1.0])) #deetrmine which of the 4 possible relative camera posistions is teh correct one # First possible rotational matrix value R = np.dot(U, W) R = np.dot(R, Vt) # Firts possible translation vector value T = U[:, 2] if (not checkInFront(R, T)): # Second choice: T = -u3 T = - U[:, 2] if (not checkInFront(R, T)): #R = U * Wt * Vt, T = u3 R = U.dot(W.T).dot(Vt) T = U[:, 2] if not checkInFront(R, T): #Only possibility T = - U[:, 2] yR1 = -math.sin([2][0]) xR1 = math.atan2(R[2][1]/math.cos(yR1), R[2][2]/math.cos(yR1)) zR1 = math.atan2(R[1][0]/math.cos(yR1), R[0][0]/math.cos(yR1)) # Decompose rotational matrix rotationX = math.atan2(R[2][1], R[2][2]) rotationY = -math.asin((R[2][0])) rotationZ = math.atan2(R[1][0], R[0][0]) print('xrotation1: ', xR1*180/np.pi, ' yrotation1: ', yR1*180/np.pi, ' zrotation: ', zR1*180/np.pi) #Output print("Rotation matrix: ", R) print("Translation vector: ", T) cv2.imshow("Current Image", BMEpilines) cv2.imshow("Best Matched Image", CurrEpilines) k = cv2.waitKey(0)
def find_correspondences(img1, img2, fundamental, xform): ''' Finds correspondences for each point between images. Args: instance (ImageInstance): The other instance to find correspondences in. Returns: dict<tuple, tuple>, a map of correspondences between (x,y) points in this image and (x,y) points in the other ''' rows, cols, _ = img1.shape # Transform to get better correspondences using cross-correlation. # # This isn't *really* rectifying the image to line up correspondences, # but we were unable to get it to work the proper way so here's the next # best thing. xform = np.linalg.inv(xform) # Reverse transform timg = cv2.warpPerspective(img2, xform, (1024, 1024)) # Find mask of the comet's silhouette def mask_for_img(img): _, mask = cv2.threshold(img, 24, 255, cv2.THRESH_BINARY) kernel = np.ones((4,4)) mask = cv2.erode(mask, kernel) mask = np.sum(mask, axis=2) return mask mask1 = mask_for_img(img1) mask2 = mask_for_img(img2) # Get points in image that lie on mask pts = [(x,y) for y in range(0, rows, sfm.CORRESPONDENCE_SKIP) for x in range(0, cols, sfm.CORRESPONDENCE_SKIP) if mask1[y,x] > 0] pts = np.float32(pts) # Find epilines lines = cv2.computeCorrespondEpilines(pts, 1, fundamental) # Scan along epilines and find correspondences correspondences = dict() def normalize(x): return (x-np.mean(x))/np.std(x) imgpad1 = np.pad(img1, sfm.WINDOW_RADIUS, 'edge') imgpad2 = np.pad(timg, sfm.WINDOW_RADIUS, 'edge') win_size = 2*sfm.WINDOW_RADIUS + 1 for i in range(0, len(pts)): a,b,c = lines[i][0] x,y = int(pts[i][0]), int(pts[i][1]) win1 = normalize(imgpad1[y:y+win_size,x:x+win_size]) best_pos = None best_corr = 0 for xx in range(0, cols): yy = int(np.round((-a*xx - c)/b)) if 0 <= yy and yy < cols and mask2[yy,xx] > 0: # Find corresponding points in transformed image t_x = int( (xform[0,0]*xx + xform[0,1]*yy + xform[0,2]) / (xform[2,0]*xx + xform[2,1]*yy + xform[2,2]) ) t_y = int( (xform[1,0]*xx + xform[1,1]*yy + xform[1,2]) / (xform[2,0]*xx + xform[2,1]*yy + xform[2,2]) ) win2 = normalize(imgpad2[t_y:t_y+win_size,t_x:t_x+win_size]) corr = np.sum(win1*win2) / win1.size if corr > best_corr: best_pos = (xx,yy) best_corr = corr if best_pos is not None: if sfm.SHOW_CORRESPONDENCE: img = Util.combine_images(img1, img2) cv2.circle(img, (x,y), 8, Util.GREEN, 2) cv2.circle(img, (best_pos[0]+1024, best_pos[1]), 8, Util.GREEN, 2) y1 = int(-c/b) y2 = int((-a*cols-c)/b) cv2.line(img, (cols, y1), (2*cols, y2), Util.BLUE, 1) img = scipy.misc.imresize(img, 0.5) cv2.imshow('debug', img) cv2.waitKey(0) cv2.destroyAllWindows() correspondences[(x,y)] = best_pos return correspondences
# ratio test to retain only the good matches for i,(m,n) in enumerate(matches): if m.distance < 0.7*n.distance: pts_left_image.append(kps_left[m.queryIdx].pt) pts_right_image.append(kps_right[m.trainIdx].pt) pts_left_image = np.float32(pts_left_image) pts_right_image = np.float32(pts_right_image) F, mask = cv2.findFundamentalMat(pts_left_image, pts_right_image, cv2.FM_LMEDS) # Selecting only the inliers pts_left_image = pts_left_image[mask.ravel()==1] pts_right_image = pts_right_image[mask.ravel()==1] # Drawing the lines on left image and the corresponding feature points on the right image lines1 = cv2.computeCorrespondEpilines(pts_right_image.reshape(-1,1,2), 2, F) lines1 = lines1.reshape(-1,3) img_left_lines, img_right_pts = draw_lines(img_left, img_right, lines1, pts_left_image, pts_right_image) # Drawing the lines on right image and the corresponding feature points on the left image lines2 = cv2.computeCorrespondEpilines(pts_left_image.reshape(-1,1,2), 1, F) lines2 = lines2.reshape(-1,3) img_right_lines, img_left_pts = draw_lines(img_right, img_left, lines2, pts_right_image, pts_left_image) cv2.imshow('Epi lines on left image', img_left_lines) cv2.imshow('Feature points on right image', img_right_pts) cv2.imshow('Epi lines on right image', img_right_lines) cv2.imshow('Feature points on left image', img_left_pts) cv2.waitKey() cv2.destroyAllWindows()
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. pointsLeft,pointsRight=[],[] for i in xrange(0,16): if i % 2 ==0: pointLeft=points[i] pointsLeft.append(pointLeft) elif i % 2 ==1: pointRight=points[i] pointsRight.append(pointRight) pointsLeft=np.array(pointsLeft, dtype=np.float32) pointsRight=np.array(pointsRight, dtype=np.float32) # <001> Estimate the Fundamental Matrix. FundMat,mask1=cv2.findFundamentalMat(pointsLeft,pointsRight) # <002> Save the Fudamental Matrix in the F attribute of the CamerasParameters class. StereoCameras.Instance.Parameters.F=FundMat # Get each point from left image. for pt in pointsLeft: # <003> Estimate the epipolar line. lineEpi=cv2.computeCorrespondEpilines(np.array([pt]), 1, StereoCameras.Instance.Parameters.F) # <004> Define the initial and final points of the line. initialP=(1280+(int(-lineEpi[0,0,2]/lineEpi[0,0,0])),0) FinalP=(1280,int(-lineEpi[0,0,2]/lineEpi[0,0,1])) # <005> Draws the epipolar line in the input image. cv2.line(self.Image, initialP, FinalP, (255,0,0)) # Get each point from right image. for pt in pointsRight: # <006> Estimate the epipolar line. lineEpi=cv2.computeCorrespondEpilines(np.array([pt]), 2, StereoCameras.Instance.Parameters.F) initialP=(int(-lineEpi[0,0,2]/lineEpi[0,0,0]),0) FinalP=(0,int(-lineEpi[0,0,2]/lineEpi[0,0,1])) cv2.line(self.Image, initialP, FinalP, (0,0,255)) # Show the final result of this process to the user. cv2.imshow("Original", self.Image) # Update the fundamental matrix flag and release the system self.hasFundamentalMatrix = True
def build_epipolar_lines(self, points, fundamental_matrix, is_right, show_lines=True): lines = cv2.computeCorrespondEpilines(points, 2 if is_right else 1, fundamental_matrix) lines = lines.reshape(-1, 3) if show_lines: self.draw_lines(self.Image, lines, points, is_right)
def externalCall(self): sift = cv2.xfeatures2d.SIFT_create() img1 = self.inputImageLeft.data img2 = self.inputImageRight.data img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) # 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) # Now we have the list of best matches from both the images. Let’s find the Fundamental Matrix. pts1 = np.int32(pts1) pts2 = np.int32(pts2) pts1 = np.float32(pts1) pts2 = np.float32(pts2) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS) if F is None: print 'F IS NONE' return print F # We select only inlier points pts1 = pts1[mask.ravel()==1] pts2 = pts2[mask.ravel()==1] # 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) cv2.imshow("img5", img5) cv2.imshow("img3", img3)
# 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.int32(pts1) pts2 = np.int32(pts2) F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS) # We select only inlier points pts1 = pts1[mask.ravel()==1] pts2 = pts2[mask.ravel()==1] # 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()
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] ]) # in place transformatino in CV2 cv2.line(img1, (x0,y0), (x1,y1), color,1) # previously img1 = blah cv2.circle(img1,tuple(pt1),5,color,-1) #print(img1) cv2.circle(img2,tuple(pt2),5,color,-1) return img1,img2 # Find epilines through the points in the right image and first image. Draw the lienes on left image lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F) # what does this do? lines1 = lines1.reshape(-1,3) img5,img6 = drawLines(img1,img2,lines1,pts1,pts2) lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2),1,F) lines2 = lines2.reshape(-1,3) img3,img4 = drawLines(img2,img1,lines2,pts2,pts1) cv2.imshow('hello',img5) cv2.imshow('hello',img3) #plt.subplot(121),plt.imshow(img5) #plt.subplot(122),plt.imshow(img3) #plt.show()
for i in range (0,len(leftImagePoints)): sampleLeftImagePoints = leftImagePoints[i] sampleRightImagePoints = rightImagePoints[i] pointsShape = sampleLeftImagePoints.shape sampleLeftImagePoints = sampleLeftImagePoints.reshape( (1,)+pointsShape ) sampleRightImagePoints = sampleRightImagePoints.reshape( (1,)+pointsShape ) pointsLeftUndistorted = cv2.undistortPoints(sampleLeftImagePoints, cameraMatrixLeft, distortionCoeffsLeft , P=cameraMatrixLeft) pointsRightUndistorted = cv2.undistortPoints(sampleRightImagePoints, cameraMatrixRight, distortionCoeffsRight , P=cameraMatrixRight) epiLinesLeft = cv2.computeCorrespondEpilines(pointsLeftUndistorted, 1, calib_F) epiLinesRight = cv2.computeCorrespondEpilines(pointsRightUndistorted, 2, calib_F) for j in range (0,epiLinesLeft.shape[0]): totalPoints += 1 totalError += math.fabs( \ sampleLeftImagePoints[0][j][0]*epiLinesRight[j][0][0] + \ sampleLeftImagePoints[0][j][1]*epiLinesRight[j][0][1] + \ epiLinesRight[j][0][2] \ ) + math.fabs( \ sampleRightImagePoints[0][j][0]*epiLinesLeft[j][0][0] + \ sampleRightImagePoints[0][j][1]*epiLinesLeft[j][0][1] + \ epiLinesLeft[j][0][2] \ ) averageEpipolarError = totalError / totalPoints