From 2ee4848d03661ff36e66a2286396b019e04c6865 Mon Sep 17 00:00:00 2001 From: Ryan Fitz-Gerald Date: Fri, 13 Mar 2026 09:53:51 -0400 Subject: [PATCH] Add initial implementation of VisualOdometry class and .gitignore file --- .gitignore | 5 ++ __init__.py | 0 visualOdometry.py | 161 ++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 166 insertions(+) create mode 100644 .gitignore create mode 100644 __init__.py create mode 100644 visualOdometry.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..37cbe99 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.claude +.venv +.vscode +train1 +claude.md \ No newline at end of file diff --git a/__init__.py b/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/visualOdometry.py b/visualOdometry.py new file mode 100644 index 0000000..4d7bd90 --- /dev/null +++ b/visualOdometry.py @@ -0,0 +1,161 @@ +from typing import Optional, Sequence + +import cv2 +import numpy as np +from matplotlib import pyplot as plt + + +class VisualOdometry: + + def __init__(self, + K: np.ndarray, + index_params: dict[str, int] = {"algorithm": 1, "trees": 5}, + search_params: dict[str, int] = {"checks": 50}): + """ Constructor + + Args: + K (np.ndarray): Camera Intrinsics Model + index_params (dict[str, int], optional): Index parameters for FLANN. Defaults to {"algorithm": 1, "trees": 5}. + search_params (dict[str, int], optional): Search parameters for FLANN. Defaults to {"checks": 50}. + """ + self.K = K + # pyright: ignore[reportAttributeAccessIssue] + self.sift = cv2.SIFT_create() + self.flann = cv2.FlannBasedMatcher( + indexParams=index_params, searchParams=search_params) # pyright: ignore[reportArgumentType] + + def extract_keypoints(self, img: cv2.typing.MatLike) -> tuple[list[cv2.KeyPoint], np.ndarray]: + """ Detects keypoints in an image + + Args: + img (cv2.typing.MatLike): _description_ + + Returns: + kp (list[cv2.KeyPoint]): list of keypoints + desc (np.ndarray): descriptor of the keypoints + """ + return self.sift.detectAndCompute(img, None) + + def match_keypoints(self, + desc1: np.ndarray, + desc2: np.ndarray, + k: int = 2) -> Sequence[Sequence[cv2.DMatch]]: + """ Matches keypoints + + Args: + desc1 (np.ndarray): image 1 keypoint description + desc2 (np.ndarray): image 2 keypoint description + k (int, optional): Defaults to 2. + + Returns: + Sequence[Sequence[cv2.DMatch]]: sequence of matches + """ + + return self.flann.knnMatch(desc1, desc2, k=k) + + def filter_matches(self, matches: Sequence[Sequence[cv2.DMatch]], distance_threshold: float = 0.7) -> list[cv2.DMatch]: + """ Filters out good keypoint matches + + Args: + matches (Sequence[Sequence[cv2.DMatch]]): list of keypoint matches + distance_threshold (float, optional): distance percent threshold for filtering. Defaults to 0.7. + + Returns: + list[cv2.DMatch]: list of good matches + """ + return [m for m, n in matches if m.distance < distance_threshold * n.distance] + + def estimate_motion(self, kp1: list[cv2.KeyPoint], kp2: list[cv2.KeyPoint], matches: list[cv2.DMatch]): + """ Estimates the motion between two images + + Args: + kp1 (list[cv2.KeyPoint]): first image keypoints + kp2 (list[cv2.KeyPoint]): second image keypoints + matches (list[cv2.DMatch]): list of keypoint matches + Returns: + TODO: Add returns + """ + + def draw_keypoint_matches(self, + img1: cv2.typing.MatLike, + kp1: list[cv2.KeyPoint], + img2: cv2.typing.MatLike, + kp2: list[cv2.KeyPoint], + matches: list[cv2.DMatch], + output_image: Optional[cv2.typing.MatLike] = None) -> cv2.typing.MatLike: + """ Generates an image drawing the keypoint matches between two images in the image + + Args: + img1 (cv2.typing.MatLike): first image + kp1 (list[cv2.KeyPoint]): first image keypoints + img2 (cv2.typing.MatLike): second image + kp2 (list[cv2.KeyPoint]): second image keypoints + matches (list[cv2.DMatch]): list of matches accepted + output_image (Optional[cv2.typing.MatLike], optional): output image buffer. If None or omitted, a new one will be created. + + Returns: + cv2.typing.MatLike: _description_ + """ + + # Draw matches + # pyright: ignore[reportArgumentType, reportCallIssue] + return cv2.drawMatches(img1, kp1, img2, kp2, matches, output_image, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) + + @staticmethod + def show_keypoint_matches(match_image: cv2.typing.MatLike) -> None: + """ Show image matches + + Args: + match_image (cv2.typing.MatLike): match image + """ + plt.figure(figsize=(15, 10)) + plt.imshow(match_image, cmap='gray') + plt.title('Matched Keypoints Between Two Images') + plt.axis('off') + plt.show() + + +def main(): + # Set Camera Intrinsics + K = np.array( + [[1389.2414846481593, 0, 962.3421649150145], + [0, 1389.2414846481593, 605.814069325842], + [0, 0, 1]], + dtype=np.float64) + + # Set Image Paths + img1_path = ".\\train1\\3d20ae25-5b29-320d-8bae-f03e9dc177b9\\ring_front_center\\ring_front_center_315975023006264672.jpg" + img2_path = ".\\train1\\3d20ae25-5b29-320d-8bae-f03e9dc177b9\\ring_front_center\\ring_front_center_315975023039564872.jpg" + + # Load images + img1 = cv2.imread(img1_path) + img2 = cv2.imread(img2_path) + + if img1 is None: + raise RuntimeError(f"Could not open or find the image {img1_path}") + + if img2 is None: + raise RuntimeError(f"Could not open or find the image {img2_path}") + + # Create an instance of the VisualOdometry class + vo = VisualOdometry(K=K) + + # Extract Keypoints + kp1, desc1 = vo.extract_keypoints(img1) + kp2, desc2 = vo.extract_keypoints(img2) + + # Match Keypoints + matches = vo.match_keypoints(desc1, desc2) + + # Filter Keypoints + good_matches = vo.filter_matches(matches) + + # Draw matches + img_matches = vo.draw_keypoint_matches(img1, kp1, img2, kp2, good_matches) + + # Show Matches + VisualOdometry.show_keypoint_matches(img_matches) + + +if __name__ == '__main__': + main()