Added motion estimation with outputs

Added Recorder pose outputs to compare calculations
Added dataset parsing library

visualOdometry.py

@@ -1,8 +1,16 @@
+from pathlib import Path
 from typing import Optional, Sequence
 
 import cv2
+
 import numpy as np
 from matplotlib import pyplot as plt
+from scipy.spatial.transform import Rotation, RigidTransform
+from sqlalchemy import create_engine, desc
+from sqlalchemy.orm import Session
+
+from util import  relative_transform
+from data_parser import *
 
 
 class VisualOdometry:
@@ -19,8 +27,8 @@ class VisualOdometry:
             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.sift = cv2.SIFT_create() # pyright: ignore[reportAttributeAccessIssue]
         self.flann = cv2.FlannBasedMatcher(
             indexParams=index_params, searchParams=search_params)  # pyright: ignore[reportArgumentType]
 
@@ -65,17 +73,34 @@ class VisualOdometry:
         """
         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
+    def estimate_motion(self, kp1: list[cv2.KeyPoint], kp2: list[cv2.KeyPoint], matches: list[cv2.DMatch]) -> RigidTransform:
+        """_summary_
 
         Args:
-            kp1 (list[cv2.KeyPoint]): first image keypoints
-            kp2 (list[cv2.KeyPoint]): second image keypoints
-            matches (list[cv2.DMatch]): list of keypoint matches
+            kp1 (list[cv2.KeyPoint]): _description_
+            kp2 (list[cv2.KeyPoint]): _description_
+            matches (list[cv2.DMatch]): _description_
+
         Returns:
-            TODO: Add returns
+            tuple[np.ndarray, np.ndarray]: _description_
         """
 
+        pts1 = np.float32([kp1[m.queryIdx].pt for m in matches]).reshape(-1, 1, 2) # pyright: ignore[reportArgumentType]
+        pts2 = np.float32([kp2[m.trainIdx].pt for m in matches]).reshape(-1, 1, 2) # pyright: ignore[reportArgumentType]
+        
+        E, _ = cv2.findEssentialMat(
+            points1=pts1, 
+            points2=pts2, 
+            cameraMatrix=self.K, 
+            method=cv2.RANSAC,
+            prob=.999,
+            threshold=1.0)
+        
+        _, R, t, _ = cv2.recoverPose(E, pts1, pts2, cameraMatrix=self.K)
+
+        return RigidTransform.from_components(translation=t.transpose(), rotation=Rotation.from_matrix(R))
+
+
     def draw_keypoint_matches(self,
                               img1: cv2.typing.MatLike,
                               kp1: list[cv2.KeyPoint],
@@ -98,9 +123,9 @@ class VisualOdometry:
         """
 
         # Draw matches
-        # pyright: ignore[reportArgumentType, reportCallIssue]
-        return cv2.drawMatches(img1, kp1, img2, kp2, matches, output_image, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
-
+        
+        return cv2.drawMatches(img1, kp1, img2, kp2, matches, output_image, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) # pyright: ignore[reportArgumentType, reportCallIssue]
+    
     @staticmethod
     def show_keypoint_matches(match_image: cv2.typing.MatLike) -> None:
         """ Show image matches
@@ -115,19 +140,29 @@ class VisualOdometry:
         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"
+def main():
+    # Create database for dataset
+    # TODO Move this to dataset library
+    engine = create_engine('sqlite:///:memory:')
+    create_tables(engine) 
+    session = Session(bind=engine)
+
+    # Import dataset
+    data_root = Path('./train1/3d20ae25-5b29-320d-8bae-f03e9dc177b9')
+    dataset = Dataset.import_dataset(data_root, session)
+
+    # Get Camera
+    camera = session.query(Camera).filter_by(name='ring_front_center').first()
+
+    if camera is None:
+        raise RuntimeError("Camera not found")
 
     # Load images
+    view1 = camera.camera_views[0]
+    view2 = camera.camera_views[1]
+    img1_path = view1.get_path()
+    img2_path = view2.get_path()
     img1 = cv2.imread(img1_path)
     img2 = cv2.imread(img2_path)
 
@@ -138,7 +173,7 @@ def main():
         raise RuntimeError(f"Could not open or find the image {img2_path}")
 
     # Create an instance of the VisualOdometry class
-    vo = VisualOdometry(K=K)
+    vo = VisualOdometry(K=camera.get_intrinsics())
 
     # Extract Keypoints
     kp1, desc1 = vo.extract_keypoints(img1)
@@ -156,6 +191,37 @@ def main():
     # Show Matches
     VisualOdometry.show_keypoint_matches(img_matches)
 
+    # Estimate pose
+    T = vo.estimate_motion(kp1, kp2, good_matches)
+
+    # Get Recoded Poses
+    T_wa = view1.timestamp.get_vehicle_pose()
+    T_wb = view2.timestamp.get_vehicle_pose()
+    T_ba = relative_transform(T_wa, T_wb)
+
+    # Print results
+    t, R = T.as_components()
+    t_ba, R_ba = T_ba.as_components()
+
+    print(f"Calculated Rotation matrix: \n{R.as_matrix()}")
+    print(f"Recorded Rotation matrix: \n{R_ba.as_matrix()}")
+    print(f"Difference Rotation matrix: \n{R_ba.as_matrix() - R.as_matrix()}")
+    print()
+
+    print(f"Calculated Euler Angles: {R.as_euler('xyz')}")
+    print(f"Recorded Euler Angles: {R_ba.as_euler('xyz')}")
+    print(f"Difference Euler Angles: {R_ba.as_euler('xyz') - R.as_euler('xyz')}")
+    print()
+
+    print(f"Calculated Quatrains: {R.as_quat()}")
+    print(f"Recorded Quatrains: {R_ba.as_quat()}")
+    print(f"Difference Quatrains: {R_ba.as_quat() - R.as_quat()}")
+    print()
+
+    print(f"Calculated Translation: {t}")
+    print(f"Recorded Translation: {t_ba}")
+    print(f"Difference Translation: {t_ba - t}")
+
 
 if __name__ == '__main__':
     main()