Initial Commit

tests/test_visual_odometry.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+"""Test visual odometry functionality."""
+
+import pytest
+import numpy as np
+import cv2
+
+
+def test_feature_detector_orb():
+    detector = cv2.ORB_create(nfeatures=500)
+    img = np.random.randint(0, 255, (480, 640), dtype=np.uint8)
+    keypoints, descriptors = detector.detectAndCompute(img, None)
+    assert keypoints is not None
+
+
+def test_feature_detector_sift():
+    detector = cv2.SIFT_create(nfeatures=500)
+    img = np.random.randint(0, 255, (480, 640), dtype=np.uint8)
+    keypoints, descriptors = detector.detectAndCompute(img, None)
+    assert keypoints is not None
+
+
+def test_feature_matching():
+    detector = cv2.ORB_create(nfeatures=100)
+    matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
+    
+    img1 = np.random.randint(0, 255, (240, 320), dtype=np.uint8)
+    img2 = np.random.randint(0, 255, (240, 320), dtype=np.uint8)
+    
+    kp1, desc1 = detector.detectAndCompute(img1, None)
+    kp2, desc2 = detector.detectAndCompute(img2, None)
+    
+    if desc1 is not None and desc2 is not None:
+        matches = matcher.knnMatch(desc1, desc2, k=2)
+        assert matches is not None
+
+
+def test_essential_matrix():
+    K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]], dtype=np.float64)
+    
+    pts1 = np.random.rand(10, 2) * 100 + 100
+    pts2 = pts1 + np.random.rand(10, 2) * 5
+    
+    pts1 = pts1.astype(np.float32)
+    pts2 = pts2.astype(np.float32)
+    
+    E, mask = cv2.findEssentialMat(pts1, pts2, K, method=cv2.RANSAC)
+    assert E is not None or mask is not None
+
+
+def test_rotation_from_scipy():
+    from scipy.spatial.transform import Rotation
+    
+    R_mat = np.eye(3)
+    r = Rotation.from_matrix(R_mat)
+    quat = r.as_quat()
+    
+    assert len(quat) == 4
+    assert np.isclose(quat[3], 1.0)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])