From a1542abc500d73947cdb77271eb2e1574b5061f9 Mon Sep 17 00:00:00 2001 From: Rick Sprague Date: Mon, 26 Aug 2024 20:57:32 -0400 Subject: [PATCH] initial --- README.md | 0 python/epipolar_geometry.py | 195 ++++++++++++++++++++++++++++++++++++ 2 files changed, 195 insertions(+) create mode 100644 README.md create mode 100644 python/epipolar_geometry.py diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/python/epipolar_geometry.py b/python/epipolar_geometry.py new file mode 100644 index 0000000..068a696 --- /dev/null +++ b/python/epipolar_geometry.py @@ -0,0 +1,195 @@ +import numpy as np +import cv2 +import matplotlib.pyplot as plt + +# ego_cv: x+ is to right, y+ downward, z+ forward +# ego_original: x+ forward, y+ to the left, z+ up +R_correct = np.array([ + [0, 0, 1], + [-1, 0, 0], + [0, -1, 0] +], np.float64) +t_correct = np.array([ + [0], + [0], + [0] +], np.float64) + +correction_matrix = np.block([ + [R_correct, t_correct], + [np.zeros((1, 3)), 1] +]) + +def skew_matrix(v): + """ + Generates a 3x3 skew-symmetric matrix from a 3D vector. + + Args: + v (np.array): A 3D vector (numpy array of shape (3,)) + + Returns: + np.array: A 3x3 skew-symmetric matrix + """ + return np.array([ + [0, -v[2], v[1]], + [v[2], 0, -v[0]], + [-v[1], v[0], 0] + ]) + +def pose_matrix(angle, translation): + def rotation_matrix(angle): + """ + Generates a 3x3 rotation matrix for a given angle (in radians) around the Z-axis. + """ + R = np.array([ + [np.cos(angle), -np.sin(angle), 0], + [np.sin(angle), np.cos(angle), 0], + [0, 0, 1] + ]) + return R + """ + Generates a 4x4 pose matrix given an angle and translation vector. + Pose matrix is in the form [R, T; 0^T, 1]. + """ + R = rotation_matrix(angle) + T = np.array(translation).reshape(3, 1) + pose = np.block([ + [R, T], + [np.zeros((1, 3)), np.array([[1]])] + ]) + + + return pose @ correction_matrix + +def apply_intrinsics(K, point_3d): + """ + Applies the intrinsic matrix to a 3D point to project it onto the image plane. + """ + point_2d_homogeneous = K @ point_3d[:3] # Apply intrinsic matrix (ignoring the last element, which is 1) + point_2d = point_2d_homogeneous / point_2d_homogeneous[2] # Convert from homogeneous to Cartesian coordinates + return point_2d + +def draw_epipolar_lines(img1, img2, pts1, pts2, F): + """ + Draws the epipolar lines on both images and marks the points. + img1, img2: Images on which to draw the lines + pts1, pts2: Points in the two images + F: Fundamental matrix + """ + # Convert points to homogeneous coordinates + pts1 = np.int32(pts1) + pts2 = np.int32(pts2) + + # Draw the epipolar lines for points in the first image + lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) + lines1 = lines1.reshape(-1, 3) + img1_with_lines = img1.copy() + + for r in lines1: + color = tuple(np.random.randint(0, 255, 3).tolist()) + x0, y0 = map(int, [0, -r[2] / r[1]]) + x1, y1 = map(int, [img1.shape[1], -(r[2] + r[0] * img1.shape[1]) / r[1]]) + img1_with_lines = cv2.line(img1_with_lines, (x0, y0), (x1, y1), color, 1) + + # Draw the epipolar lines for points in the second image + lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) + lines2 = lines2.reshape(-1, 3) + img2_with_lines = img2.copy() + + for r in lines2: + color = tuple(np.random.randint(0, 255, 3).tolist()) + x0, y0 = map(int, [0, -r[2] / r[1]]) + x1, y1 = map(int, [img2.shape[1], -(r[2] + r[0] * img2.shape[1]) / r[1]]) + img2_with_lines = cv2.line(img2_with_lines, (x0, y0), (x1, y1), color, 1) + + return img1_with_lines, img2_with_lines + +def find_epipoles(F): + """ + Finds the epipoles in the image given the Fundamental matrix. + """ + # Compute the left epipole + _, _, Vt = np.linalg.svd(F.T) + e1 = Vt[-1, :] + e1 = e1 / e1[2] # Normalize + + # Compute the right epipole + _, _, Vt = np.linalg.svd(F) + e2 = Vt[-1, :] + e2 = e2 / e2[2] # Normalize + + return e1, e2 + +def main(): + # Define pose matrices using rotation around Z-axis and translation + verge = 30 + P1 = pose_matrix(np.radians(-verge), [0, 1, 0]) # P1 rotated -30 degrees and translated (0, 1, 0) + P2 = pose_matrix(np.radians(+verge), [0, -1, 0]) # P2 rotated 30 degrees and translated (0, -1, 0) + D = np.linalg.inv(P1) @ P2 + + # Example 3D landmark point in homogeneous coordinates + X = np.array([10, 0, 0, 1]) + + # Project the 3D point into the two camera images + x1_ego = np.linalg.inv(P1) @ X # 3D point in camera 1's coordinate system + x2_ego = np.linalg.inv(P2) @ X # 3D point in camera 2's coordinate system + print("X: ", X) + print("x1_ego: ", x1_ego) + print("x2_ego: ", x2_ego) + + # Intrinsic matrix (example) + K = np.array([ + [800, 0, 250], + [0, 800, 250], + [0, 0, 1] + ]) + Kinv = np.linalg.inv(K) + + + # Apply the intrinsic matrix to get 2D points + x1_2d = apply_intrinsics(K, x1_ego) + x2_2d = apply_intrinsics(K, x2_ego) + + print("x1_2d: ", x1_2d) + print("x2_2d: ", x2_2d) + + # Fundamental matrix (example, normally computed from camera matrices) + E = skew_matrix(D[:3, 3]) @ D[:3, :3] + F = Kinv.T @ E @ Kinv + + # Example images + img1 = np.ones((500, 500, 3), dtype=np.uint8) * 255 + img2 = np.ones((500, 500, 3), dtype=np.uint8) * 255 + + # Draw epipolar lines + pts1 = np.array([x1_2d[:2]]) + pts2 = np.array([x2_2d[:2]]) + + # Find epipoles + e1, e2 = find_epipoles(F) + print("e1: " , e1) + print("e2: " , e2) + + img1_with_lines, img2_with_lines = draw_epipolar_lines(img1, img2, pts1, pts2, F) + + # Plot the results + plt.figure(figsize=(10, 5)) + plt.subplot(1, 2, 1) + plt.imshow(img1_with_lines) + plt.scatter([e1[0]], [e1[1]], color='red', marker='x', label='Epipole') # Epipole in the first image + plt.scatter([x1_2d[0]], [x1_2d[1]], color='blue', marker='o', label='Landmark') # Landmark projection + plt.title("Image 1 with Epipolar Lines") + plt.legend() + + plt.subplot(1, 2, 2) + plt.imshow(img2_with_lines) + plt.scatter([e2[0]], [e2[1]], color='red', marker='x', label='Epipole') # Epipole in the second image + plt.scatter([x2_2d[0]], [x2_2d[1]], color='blue', marker='o', label='Landmark') # Landmark projection + plt.title("Image 2 with Epipolar Lines") + plt.legend() + + plt.show() + +if __name__ == "__main__": + main() +