mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 22:46:36 +00:00
Yuzviak
103 lines
3.1 KiB
Python
103 lines
3.1 KiB
Python
"""Tests for Sequential Visual Odometry (F07)."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from gps_denied.core.models import ModelManager
|
|
from gps_denied.core.vo import SequentialVisualOdometry
|
|
from gps_denied.schemas.flight import CameraParameters
|
|
from gps_denied.schemas.vo import Features, Matches
|
|
|
|
|
|
@pytest.fixture
|
|
def vo():
|
|
manager = ModelManager()
|
|
return SequentialVisualOdometry(manager)
|
|
|
|
|
|
@pytest.fixture
|
|
def cam_params():
|
|
return CameraParameters(
|
|
focal_length=5.0,
|
|
sensor_width=6.4,
|
|
sensor_height=4.8,
|
|
resolution_width=640,
|
|
resolution_height=480,
|
|
principal_point=(320.0, 240.0)
|
|
)
|
|
|
|
|
|
def test_extract_features(vo):
|
|
img = np.zeros((480, 640, 3), dtype=np.uint8)
|
|
features = vo.extract_features(img)
|
|
|
|
assert isinstance(features, Features)
|
|
assert features.keypoints.shape == (500, 2)
|
|
assert features.descriptors.shape == (500, 256)
|
|
|
|
|
|
def test_match_features(vo):
|
|
f1 = Features(
|
|
keypoints=np.random.rand(100, 2),
|
|
descriptors=np.random.rand(100, 256),
|
|
scores=np.random.rand(100)
|
|
)
|
|
f2 = Features(
|
|
keypoints=np.random.rand(100, 2),
|
|
descriptors=np.random.rand(100, 256),
|
|
scores=np.random.rand(100)
|
|
)
|
|
|
|
matches = vo.match_features(f1, f2)
|
|
assert isinstance(matches, Matches)
|
|
assert matches.matches.shape == (100, 2)
|
|
|
|
|
|
def test_estimate_motion_insufficient_matches(vo, cam_params):
|
|
matches = Matches(
|
|
matches=np.zeros((5, 2)),
|
|
scores=np.zeros(5),
|
|
keypoints1=np.zeros((5, 2)),
|
|
keypoints2=np.zeros((5, 2))
|
|
)
|
|
|
|
# Less than 8 points should return None
|
|
motion = vo.estimate_motion(matches, cam_params)
|
|
assert motion is None
|
|
|
|
|
|
def test_estimate_motion_synthetic(vo, cam_params):
|
|
# To reliably test compute_relative_pose, we create points strictly satisfying epipolar constraint
|
|
# Simple straight motion: Add a small shift on X axis
|
|
n_pts = 100
|
|
pts1 = np.random.rand(n_pts, 2) * 400 + 100
|
|
pts2 = pts1 + np.array([10.0, 0.0]) # moving 10 pixels right
|
|
|
|
matches = Matches(
|
|
matches=np.column_stack([np.arange(n_pts), np.arange(n_pts)]),
|
|
scores=np.ones(n_pts),
|
|
keypoints1=pts1,
|
|
keypoints2=pts2
|
|
)
|
|
|
|
motion = vo.estimate_motion(matches, cam_params)
|
|
assert motion is not None
|
|
assert motion.inlier_count > 20
|
|
assert motion.translation.shape == (3,)
|
|
assert motion.rotation.shape == (3, 3)
|
|
|
|
|
|
def test_compute_relative_pose(vo, cam_params):
|
|
img1 = np.zeros((480, 640, 3), dtype=np.uint8)
|
|
img2 = np.zeros((480, 640, 3), dtype=np.uint8)
|
|
|
|
# Given the random nature of our mock, OpenCV's findEssentialMat will likely find 0 inliers
|
|
# or fail. We expect compute_relative_pose to gracefully return None or low confidence.
|
|
pose = vo.compute_relative_pose(img1, img2, cam_params)
|
|
|
|
if pose is not None:
|
|
assert pose.translation.shape == (3,)
|
|
assert pose.rotation.shape == (3, 3)
|
|
# Because we randomize points in the mock manager, inliers will be extremely low
|
|
assert pose.tracking_good is False
|