Download samples/python/essential_mat_reconstr.py from tgollogly/test: direct link, hf CLI and curl.
- Browser
- Download file 5.94 kB
-
https://huggingface.co/tgollogly/test/resolve/main/samples/python/essential_mat_reconstr.py
- Command line
-
hf download hf://tgollogly/test/samples/python/essential_mat_reconstr.py
-
curl -L -o essential_mat_reconstr.py https://huggingface.co/tgollogly/test/resolve/main/samples/python/essential_mat_reconstr.py
5.94 kB
| import numpy as np, cv2 as cv, matplotlib.pyplot as plt, time, sys, os | |
| from mpl_toolkits.mplot3d import axes3d, Axes3D | |
| def getEpipolarError(F, pts1_, pts2_, inliers): | |
| pts1 = np.concatenate((pts1_.T, np.ones((1, pts1_.shape[0]))))[:,inliers] | |
| pts2 = np.concatenate((pts2_.T, np.ones((1, pts2_.shape[0]))))[:,inliers] | |
| lines2 = np.dot(F , pts1) | |
| lines1 = np.dot(F.T, pts2) | |
| return np.median((np.abs(np.sum(pts1 * lines1, axis=0)) / np.sqrt(lines1[0,:]**2 + lines1[1,:]**2) + | |
| np.abs(np.sum(pts2 * lines2, axis=0)) / np.sqrt(lines2[0,:]**2 + lines2[1,:]**2))/2) | |
| if __name__ == '__main__': | |
| if len(sys.argv) < 3: | |
| print("Path to data file and directory to image files are missing!\nData file must have" | |
| " format:\n--------------\n image_name_1\nimage_name_2\nk11 k12 k13\n0 k22 k23\n" | |
| "0 0 1\n--------------\nIf image_name_{1,2} are not in the same directory as " | |
| "the data file then add argument with directory to image files.\nFor example: " | |
| "python essential_mat_reconstr.py essential_mat_data.txt ./") | |
| exit(1) | |
| else: | |
| data_file = sys.argv[1] | |
| image_dir = sys.argv[2] | |
| if not os.path.isfile(data_file): | |
| print('Incorrect path to data file!') | |
| exit(1) | |
| with open(data_file, 'r') as f: | |
| image1 = cv.imread(image_dir+f.readline()[:-1]) # remove '\n' | |
| image2 = cv.imread(image_dir+f.readline()[:-1]) | |
| K = np.array([[float(x) for x in f.readline().split(' ')], | |
| [float(x) for x in f.readline().split(' ')], | |
| [float(x) for x in f.readline().split(' ')]]) | |
| if image1 is None or image2 is None: | |
| print('Incorrect directory to images!') | |
| exit(1) | |
| if K.shape != (3,3): | |
| print('Intrinsic matrix has incorrect format!') | |
| exit(1) | |
| print('find keypoints and compute descriptors') | |
| detector = cv.SIFT_create(nfeatures=20000) | |
| keypoints1, descriptors1 = detector.detectAndCompute(cv.cvtColor(image1, cv.COLOR_BGR2GRAY), None) | |
| keypoints2, descriptors2 = detector.detectAndCompute(cv.cvtColor(image2, cv.COLOR_BGR2GRAY), None) | |
| matcher = cv.FlannBasedMatcher(dict(algorithm=0, trees=5), dict(checks=32)) | |
| print('match with FLANN, size of descriptors', descriptors1.shape, descriptors2.shape) | |
| matches_vector = matcher.knnMatch(descriptors1, descriptors2, k=2) | |
| print('find good keypoints') | |
| pts1 = []; pts2 = [] | |
| for m in matches_vector: | |
| # compare best and second match using Lowe ratio test | |
| if m[0].distance / m[1].distance < 0.75: | |
| pts1.append(keypoints1[m[0].queryIdx].pt) | |
| pts2.append(keypoints2[m[0].trainIdx].pt) | |
| pts1 = np.array(pts1); pts2 = np.array(pts2) | |
| print('points size', pts1.shape[0]) | |
| print('Essential matrix RANSAC') | |
| start = time.time() | |
| E, inliers = cv.findEssentialMat(pts1, pts2, K, cv.RANSAC, 0.999, 1.0) | |
| print('RANSAC time', time.time() - start, 'seconds') | |
| print('Median error to epipolar lines', getEpipolarError | |
| (np.dot(np.linalg.inv(K).T, np.dot(E, np.linalg.inv(K))), pts1, pts2, inliers.squeeze()), | |
| 'number of inliers', inliers.sum()) | |
| print('Decompose essential matrix') | |
| R1, R2, t = cv.decomposeEssentialMat(E) | |
| # Assume relative pose. Fix the first camera | |
| P1 = np.concatenate((K, np.zeros((3,1))), axis=1) # K [I | 0] | |
| P2s = [np.dot(K, np.concatenate((R1, t), axis=1)), # K[R1 | t] | |
| np.dot(K, np.concatenate((R1, -t), axis=1)), # K[R1 | -t] | |
| np.dot(K, np.concatenate((R2, t), axis=1)), # K[R2 | t] | |
| np.dot(K, np.concatenate((R2, -t), axis=1))] # K[R2 | -t] | |
| obj_pts_per_cam = [] | |
| # enumerate over all P2 projection matrices | |
| for cam_idx, P2 in enumerate(P2s): | |
| obj_pts = [] | |
| for i, (pt1, pt2) in enumerate(zip(pts1, pts2)): | |
| if not inliers[i]: | |
| continue | |
| # find object point by triangulation of image points by projection matrices | |
| obj_pt = cv.triangulatePoints(P1, P2, pt1, pt2) | |
| obj_pt /= obj_pt[3] | |
| # check if reprojected point has positive depth | |
| if obj_pt[2] > 0: | |
| obj_pts.append([obj_pt[0], obj_pt[1], obj_pt[2]]) | |
| obj_pts_per_cam.append(obj_pts) | |
| best_cam_idx = np.array([len(obj_pts_per_cam[0]),len(obj_pts_per_cam[1]), | |
| len(obj_pts_per_cam[2]),len(obj_pts_per_cam[3])]).argmax() | |
| max_pts = len(obj_pts_per_cam[best_cam_idx]) | |
| print('Number of object points', max_pts) | |
| # filter object points to have reasonable depth | |
| MAX_DEPTH = 6. | |
| obj_pts = [] | |
| for pt in obj_pts_per_cam[best_cam_idx]: | |
| if pt[2] < MAX_DEPTH: | |
| obj_pts.append(pt) | |
| obj_pts = np.array(obj_pts).reshape(len(obj_pts), 3) | |
| # visualize image points | |
| for i, (pt1, pt2) in enumerate(zip(pts1, pts2)): | |
| if inliers[i]: | |
| cv.circle(image1, (int(pt1[0]), int(pt1[1])), 7, (255,0,0), -1) | |
| cv.circle(image2, (int(pt2[0]), int(pt2[1])), 7, (255,0,0), -1) | |
| # concatenate two images | |
| image1 = np.concatenate((image1, image2), axis=1) | |
| # resize concatenated image | |
| new_img_size = 1200. * 800. | |
| image1 = cv.resize(image1, (int(np.sqrt(image1.shape[1] * new_img_size / image1.shape[0])), | |
| int(np.sqrt (image1.shape[0] * new_img_size / image1.shape[1])))) | |
| # plot object points | |
| fig = plt.figure(figsize=(13.0, 11.0)) | |
| ax = fig.add_subplot(111, projection='3d') | |
| ax.set_aspect('equal') | |
| ax.scatter(obj_pts[:,0], obj_pts[:,1], obj_pts[:,2], c='r', marker='o', s=3) | |
| ax.set_xlabel('x') | |
| ax.set_ylabel('y') | |
| ax.set_zlabel('depth') | |
| ax.view_init(azim=-80, elev=110) | |
| # save figures | |
| cv.imshow("matches", image1) | |
| cv.imwrite('matches_E.png', image1) | |
| plt.savefig('reconstruction_3D.png') | |
| cv.waitKey(0) | |
| plt.show() | |