Add two more examples

Author: sunglok

README.md

@@ -73,7 +73,6 @@ In addition to tutorial slides, example codes are provided in the purpose of edu
   * Structure-from-Motion (SfM)
     * Global SfM
     * Incremental SfM
-  * COLMAP
 * **Section 7. Visual SLAM and Odometry**
 
 
@@ -84,8 +83,8 @@ In addition to tutorial slides, example codes are provided in the purpose of edu
 
 
 ### Authors
-* [Sunglok Choi](http://sites.google.com/site/sunglok/) (sunglok@hanmail.net)
-* [JunHyeok Choi](https://mint-lab.github.io/members/) (dkwnsgur12@gmail.com)
+* [Sunglok Choi](https://mint-lab.github.io/sunglok/)
+* [JunHyeok Choi](https://github.com/cjh1995-ros)
 
 
 

examples/epipolar_line_visualization.py

@@ -0,0 +1,63 @@
+import numpy as np
+import cv2 as cv
+import random
+
+def mouse_event_handler(event, x, y, flags, param):
+    if event == cv.EVENT_LBUTTONDOWN:
+        param.append((x, y))
+
+def draw_straight_line(img, line, color, thickness=1):
+    assert img.ndim >= 2
+    h, w, *_ = img.shape
+    a, b, c = line # Line: ax + by + c = 0
+    if abs(a) > abs(b):
+        pt1 = (int(c / -a), 0)
+        pt2 = (int((b*h + c) / -a), h)
+    else:
+        pt1 = (0, int(c / -b))
+        pt2 = (w, int((a*w + c) / -b))
+    cv.line(img, pt1, pt2, color, thickness)
+
+if __name__ == '__main__':
+    # Load two images
+    img1 = cv.imread('../data/KITTI07/image_0/000000.png', cv.IMREAD_COLOR)
+    img2 = cv.imread('../data/KITTI07/image_0/000023.png', cv.IMREAD_COLOR)
+    assert (img1 is not None) and (img2 is not None), 'Cannot read the given images'
+    # Note) `F` is derived from `fundamental_mat_estimation.py`.
+    F = np.array([[ 3.34638533e-07,  7.58547151e-06, -2.04147752e-03],
+                  [-5.83765868e-06,  1.36498636e-06,  2.67566877e-04],
+                  [ 1.45892349e-03, -4.37648316e-03,  1.00000000e+00]])
+
+    # Register event handlers and show images
+    wnd1_name, wnd2_name = 'Epipolar Line: Image #1', 'Epipolar Line: Image #2'
+    img1_pts, img2_pts = [], []
+    cv.namedWindow(wnd1_name)
+    cv.namedWindow(wnd2_name)
+    cv.setMouseCallback(wnd1_name, mouse_event_handler, img1_pts)
+    cv.setMouseCallback(wnd2_name, mouse_event_handler, img2_pts)
+    cv.imshow(wnd1_name, img1)
+    cv.imshow(wnd2_name, img2)
+
+    # Get a point from a image and draw its correponding epipolar line on the other image
+    while True:
+        if len(img1_pts) > 0:
+            for x, y in img1_pts:
+                color = (random.randrange(256), random.randrange(256), random.randrange(256))
+                cv.circle(img1, (x, y), 4, color, -1)
+                epipolar_line = F @ [[x], [y], [1]]
+                draw_straight_line(img2, epipolar_line, color, 2)
+            img1_pts.clear()
+        if len(img2_pts) > 0:
+            for x, y in img2_pts:
+                color = (random.randrange(256), random.randrange(256), random.randrange(256))
+                cv.circle(img2, (x, y), 4, color, -1)
+                epipolar_line = F.T @ [[x], [y], [1]]
+                draw_straight_line(img1, epipolar_line, color, 2)
+            img2_pts.clear()
+        cv.imshow(wnd2_name, img2)
+        cv.imshow(wnd1_name, img1)
+        key = cv.waitKey(10)
+        if key == 27: # ESC
+            break
+
+    cv.destroyAllWindows()

examples/fundamental_mat_estimation.py

@@ -0,0 +1,44 @@
+import numpy as np
+import cv2 as cv
+
+# Load two images
+img1 = cv.imread('../data/KITTI07/image_0/000000.png')
+img2 = cv.imread('../data/KITTI07/image_0/000023.png')
+assert (img1 is not None) and (img2 is not None), 'Cannot read the given images'
+f, cx, cy = 707.0912, 601.8873, 183.1104 # From the KITTI dataset
+K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
+
+# Retrieve matching points
+brisk = cv.BRISK_create()
+keypoints1, descriptors1 = brisk.detectAndCompute(img1, None)
+keypoints2, descriptors2 = brisk.detectAndCompute(img2, None)
+
+fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming')
+match = fmatcher.match(descriptors1, descriptors2)
+
+# Calculate the fundamental matrix
+pts1, pts2 = [], []
+for i in range(len(match)):
+    pts1.append(keypoints1[match[i].queryIdx].pt)
+    pts2.append(keypoints2[match[i].trainIdx].pt)
+pts1 = np.array(pts1, dtype=np.float32)
+pts2 = np.array(pts2, dtype=np.float32)
+F, inlier_mask = cv.findFundamentalMat(pts1, pts2, cv.FM_RANSAC, 0.5, 0.999)
+print(f'* F = {F}')
+print(f'* The number of inliers = {sum(inlier_mask.ravel())}')
+
+# Extract relative camera pose between two images
+E = K.T @ F @ K
+positive_num, R, t, positive_mask = cv.recoverPose(E, pts1, pts2, K, mask=inlier_mask)
+print(f'* R = {R}')
+print(f'* t = {t}')
+print(f'* The position of Image #2 = {-R.T @ t}') # [-0.57, 0.09, 0.82]
+print(f'* The number of positive-depth inliers = {sum(positive_mask.ravel())}')
+
+# Show the matched images
+img_matched = cv.drawMatches(img1, keypoints1, img2, keypoints2, match, None, None, None,
+                             matchesMask=inlier_mask.ravel().tolist()) # Remove `matchesMask` if you want to show all putative matches
+cv.namedWindow('Fundamental Matrix Estimation', cv.WINDOW_NORMAL)
+cv.imshow('Fundamental Matrix Estimation', img_matched)
+cv.waitKey(0)
+cv.destroyAllWindows()