64 lines
1.8 KiB
Python
64 lines
1.8 KiB
Python
#!/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'])
|