Initial Commit

tests/__init__.py

@@ -0,0 +1 @@
+"""Tests package."""

tests/test_camera.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+"""Test camera processing."""
+
+import pytest
+import numpy as np
+import cv2
+
+
+def test_image_processing():
+    img = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
+    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+    assert gray.shape == (480, 640)
+
+
+def test_clahe():
+    gray = np.random.randint(0, 255, (480, 640), dtype=np.uint8)
+    clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
+    enhanced = clahe.apply(gray)
+    assert enhanced.shape == gray.shape
+
+
+def test_undistort_maps():
+    K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]], dtype=np.float64)
+    dist = np.zeros(5)
+    size = (640, 480)
+    
+    new_K, roi = cv2.getOptimalNewCameraMatrix(K, dist, size, alpha=0)
+    map1, map2 = cv2.initUndistortRectifyMap(K, dist, None, new_K, size, cv2.CV_16SC2)
+    
+    assert map1 is not None
+    assert map2 is not None
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

tests/test_installation.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+"""Test installation and imports."""
+
+import pytest
+import sys
+
+
+def test_python_version():
+    assert sys.version_info.major == 3
+    assert sys.version_info.minor >= 10
+
+
+def test_numpy_import():
+    import numpy as np
+    assert np.__version__
+
+
+def test_opencv_import():
+    import cv2
+    assert cv2.__version__
+
+
+def test_scipy_import():
+    from scipy.spatial.transform import Rotation
+    r = Rotation.from_euler('xyz', [0, 0, 0])
+    assert r is not None
+
+
+def test_shapely_import():
+    from shapely.geometry import Point, Polygon
+    p = Point(0, 0)
+    assert p is not None
+
+
+def test_filterpy_import():
+    from filterpy.kalman import ExtendedKalmanFilter
+    ekf = ExtendedKalmanFilter(dim_x=3, dim_z=2)
+    assert ekf is not None
+
+
+def test_transforms3d_import():
+    import transforms3d
+    assert transforms3d is not None
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

tests/test_position_estimator.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+"""Test position estimator and EKF."""
+
+import pytest
+import numpy as np
+
+
+def test_ekf_initialization():
+    from filterpy.kalman import ExtendedKalmanFilter
+    
+    ekf = ExtendedKalmanFilter(dim_x=9, dim_z=6)
+    ekf.x = np.zeros(9)
+    ekf.P = np.eye(9)
+    ekf.Q = np.eye(9) * 0.1
+    ekf.R = np.eye(6) * 0.1
+    
+    assert ekf.x.shape == (9,)
+    assert ekf.P.shape == (9, 9)
+
+
+def test_state_transition():
+    dt = 0.02
+    x = np.array([1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.0, 0.0, 0.0])
+    
+    F = np.eye(9)
+    F[0, 3] = dt
+    F[1, 4] = dt
+    F[2, 5] = dt
+    
+    x_new = F @ x
+    
+    assert np.isclose(x_new[0], 1.0 + 0.1 * dt)
+    assert np.isclose(x_new[1], 2.0 + 0.2 * dt)
+    assert np.isclose(x_new[2], 3.0 + 0.3 * dt)
+
+
+def test_measurement_update():
+    from filterpy.kalman import ExtendedKalmanFilter
+    
+    ekf = ExtendedKalmanFilter(dim_x=6, dim_z=3)
+    ekf.x = np.zeros(6)
+    ekf.P = np.eye(6)
+    ekf.Q = np.eye(6) * 0.1
+    ekf.R = np.eye(3) * 0.1
+    ekf.F = np.eye(6)
+    
+    H = np.zeros((3, 6))
+    H[0, 0] = 1.0
+    H[1, 1] = 1.0
+    H[2, 2] = 1.0
+    
+    z = np.array([1.0, 2.0, 3.0])
+    
+    ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x)
+    
+    assert ekf.x[0] > 0
+
+
+def test_weighted_average():
+    weights = [0.6, 0.3, 0.1]
+    values = [np.array([1, 0, 0]), np.array([0, 1, 0]), np.array([0, 0, 1])]
+    
+    total = sum([w * v for w, v in zip(weights, values)])
+    
+    assert np.isclose(total[0], 0.6)
+    assert np.isclose(total[1], 0.3)
+    assert np.isclose(total[2], 0.1)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

tests/test_uav_control.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""Test UAV control logic."""
+
+import pytest
+import numpy as np
+
+
+def test_waypoint_distance():
+    current = np.array([0, 0, 5])
+    target = np.array([10, 0, 5])
+    distance = np.linalg.norm(target - current)
+    assert np.isclose(distance, 10.0)
+
+
+def test_velocity_command():
+    current = np.array([0, 0, 5])
+    target = np.array([10, 0, 5])
+    
+    direction = target - current
+    distance = np.linalg.norm(direction)
+    direction = direction / distance
+    
+    max_vel = 2.0
+    speed = min(max_vel, distance * 0.5)
+    velocity = direction * speed
+    
+    assert np.isclose(velocity[0], max_vel)
+    assert np.isclose(velocity[1], 0.0)
+
+
+def test_position_hold():
+    current = np.array([5.0, 3.0, 10.0])
+    target = current.copy()
+    
+    error = np.linalg.norm(target - current)
+    assert np.isclose(error, 0.0)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

tests/test_ugv_control.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+"""Test UGV control logic."""
+
+import pytest
+import numpy as np
+
+
+def test_differential_drive():
+    target_angle = np.pi / 4
+    current_yaw = 0
+    
+    angle_error = target_angle - current_yaw
+    kp_angular = 1.5
+    max_angular = 1.0
+    
+    angular_cmd = np.clip(kp_angular * angle_error, -max_angular, max_angular)
+    assert np.isclose(angular_cmd, 1.0)
+
+
+def test_angle_normalization():
+    def normalize(angle):
+        while angle > np.pi:
+            angle -= 2 * np.pi
+        while angle < -np.pi:
+            angle += 2 * np.pi
+        return angle
+    
+    assert np.isclose(normalize(3 * np.pi), np.pi)
+    assert np.isclose(normalize(-3 * np.pi), np.pi)
+
+
+def test_goal_distance_2d():
+    current = np.array([0, 0])
+    target = np.array([3, 4])
+    
+    distance = np.linalg.norm(target - current)
+    assert np.isclose(distance, 5.0)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

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'])