Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rudimentary geolocation #223

Draft
wants to merge 13 commits into
base: main
Choose a base branch
from
+321 −0
Draft

Rudimentary geolocation #223

Changes from 1 commit
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
c943cc0
Rudimentary Geolocation
asterbot Nov 3, 2024
6d0202b
Rudimentary Geolocation
asterbot Nov 3, 2024
3b6efbc
Removed unecessary folder
asterbot Nov 3, 2024
d7e7bc6
Figured out correct matrix and found required unit tests. Need to mak…
asterbot Nov 6, 2024
817e7a3
rebase with main
asterbot Nov 6, 2024
647311e
fixed unit test imports to align with common-lib refactor
asterbot Nov 6, 2024
004db84
merge main
asterbot Nov 9, 2024
44fba8c
Refactored to make it inherit from geolocation
asterbot Nov 12, 2024
f4cf186
Linters
asterbot Nov 12, 2024
fe9631a
Running black linter
asterbot Nov 12, 2024
f6d0f9d
Pylint fix
asterbot Nov 12, 2024
2bd09a8
black linting
asterbot Nov 12, 2024
87a7c86
Removed unecessary unit tests
asterbot Nov 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Loading
Loading status checks…
Removed unecessary unit tests
  • Loading branch information
@asterbot
asterbot committed Nov 12, 2024
commit 87a7c86af47f627a2bf96e95b0d181dee71fef2e
764 changes: 47 additions & 717 deletions tests/unit/test_rudimentary_geolocation.py
View file
Original file line number Diff line number Diff line change
@@ -21,6 +21,7 @@
# Test functions use test fixture signature names and access class privates
# No enable
# pylint: disable=protected-access,redefined-outer-name
# pylint: disable=duplicate-code


@pytest.fixture
@@ -59,111 +60,6 @@ def basic_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type:
yield locator # type: ignore


@pytest.fixture
def intermediate_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore
"""
Downwards pointing camera offset towards front of drone.
"""
result, camera_intrinsics = camera_properties.CameraIntrinsics.create(
2000,
2000,
np.pi / 2,
np.pi / 2,
)
assert result
assert camera_intrinsics is not None

result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create(
(1.0, 0.0, 0.0),
(0.0, -np.pi / 2, 0.0),
)
assert result
assert camera_extrinsics is not None

result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

result, locator = rudimentary_geolocation.RudimentaryGeolocation.create(
camera_intrinsics,
camera_extrinsics,
test_logger,
)
assert result
assert locator is not None

yield locator # type: ignore


@pytest.fixture
def advanced_locator() -> rudimentary_geolocation.RudimentaryGeolocation: # type: ignore
"""
Camera angled at 75° upward.
Drone is expected to rotate it downwards.
"""
result, camera_intrinsics = camera_properties.CameraIntrinsics.create(
2000,
2000,
np.pi / 3,
np.pi / 3,
)
assert result
assert camera_intrinsics is not None

result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create(
(1.0, 0.0, 0.0),
(0.0, np.pi * 5 / 12, np.pi / 2),
)
assert result
assert camera_extrinsics is not None

result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

result, locator = rudimentary_geolocation.RudimentaryGeolocation.create(
camera_intrinsics,
camera_extrinsics,
test_logger,
)
assert result
assert locator is not None

yield locator # type: ignore


@pytest.fixture
def detection_bottom_right_point() -> detections_and_time.Detection: # type: ignore
"""
Bounding box is a single point.
"""
result, detection = detections_and_time.Detection.create(
np.array([2000.0, 2000.0, 2000.0, 2000.0], dtype=np.float32),
0,
0.01,
)
assert result
assert detection is not None

yield detection # type: ignore


@pytest.fixture
def detection_centre_left_point() -> detections_and_time.Detection: # type: ignore
"""
Bounding box is a single point.
"""
result, detection = detections_and_time.Detection.create(
np.array([0.0, 1000.0, 0.0, 1000.0], dtype=np.float32),
0,
0.1,
)
assert result
assert detection is not None

yield detection # type: ignore


@pytest.fixture
def detection_1() -> detections_and_time.Detection: # type: ignore
"""
@@ -196,618 +92,6 @@ def detection_2() -> detections_and_time.Detection: # type: ignore
yield detection # type: ignore


@pytest.fixture
def affine_matrix() -> np.ndarray: # type: ignore
"""
3x3 homogeneous.
"""
# fmt: off
matrix = np.array(
[
[0.0, 1.0, -1.0],
[2.0, 0.0, -1.0],
[0.0, 0.0, 1.0],
],
dtype=np.float32,
)
# fmt: on

yield matrix # type: ignore


@pytest.fixture
def non_affine_matrix() -> np.ndarray: # type: ignore
"""
3x3 homogeneous.
"""
# fmt: off
matrix = np.array(
[
[0.0, 1.0, -1.0],
[2.0, 0.0, -1.0],
[1.0, 1.0, 1.0],
],
dtype=np.float32,
)
# fmt: on

yield matrix # type: ignore


class TestRudimentaryGeolocationCreate:
"""
Test constructor.
"""

def test_normal(self) -> None:
"""
Successful construction.
"""
result, camera_intrinsics = camera_properties.CameraIntrinsics.create(
2000,
2000,
np.pi / 2,
np.pi / 2,
)
assert result
assert camera_intrinsics is not None

result, camera_extrinsics = camera_properties.CameraDroneExtrinsics.create(
(0.0, 0.0, 0.0),
(0.0, 0.0, 0.0),
)
assert result
assert camera_extrinsics is not None

result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

result, locator = rudimentary_geolocation.RudimentaryGeolocation.create(
camera_intrinsics,
camera_extrinsics,
test_logger,
)
assert result
assert locator is not None


class TestGroundIntersection:
"""
Test where vector intersects with ground.
"""

def test_above_origin_directly_down(self) -> None:
"""
Above origin, directly down.
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32)
vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32)

expected = np.array([0.0, 0.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_camera_in_world_position,
vec_down,
test_logger,
)

# Test
assert result
assert actual is not None
np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE)

def test_non_origin_directly_down(self) -> None:
"""
Directly down.
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_camera_in_world_position = np.array([100.0, -100.0, -100.0], dtype=np.float32)
vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32)

expected = np.array([100.0, -100.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_camera_in_world_position,
vec_down,
test_logger,
)

# Test
assert result
assert actual is not None
np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE)

def test_above_origin_angled_down(self) -> None:
"""
Above origin, angled down towards positive.
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32)
vec_down = np.array([1.0, 1.0, 1.0], dtype=np.float32)

expected = np.array([100.0, 100.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_camera_in_world_position,
vec_down,
test_logger,
)

# Test
assert result
assert actual is not None
np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE)

def test_non_origin_angled_down(self) -> None:
"""
Angled down towards origin.
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_camera_in_world_position = np.array([100.0, -100.0, -100.0], dtype=np.float32)
vec_down = np.array([-1.0, 1.0, 1.0], dtype=np.float32)

expected = np.array([0.0, 0.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_camera_in_world_position,
vec_down,
test_logger,
)

# Test
assert result
assert actual is not None
np.testing.assert_almost_equal(actual, expected, decimal=FLOAT_PRECISION_TOLERANCE)

def test_bad_almost_horizontal(self) -> None:
"""
False, None .
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32)
vec_horizontal = np.array([10.0, 0.0, 1.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_camera_in_world_position,
vec_horizontal,
test_logger,
)

# Test
assert not result
assert actual is None

def test_bad_upwards(self) -> None:
"""
False, None .
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_camera_in_world_position = np.array([0.0, 0.0, -100.0], dtype=np.float32)
vec_up = np.array([0.0, 0.0, -1.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_camera_in_world_position,
vec_up,
test_logger,
)

# Test
assert not result
assert actual is None

def test_bad_underground(self) -> None:
"""
False, None .
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

vec_underground = np.array([0.0, 0.0, 1.0], dtype=np.float32)
vec_down = np.array([0.0, 0.0, 1.0], dtype=np.float32)

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__ground_intersection_from_vector( # type: ignore
vec_underground,
vec_down,
test_logger,
)

# Test
assert not result
assert actual is None


class TestPerspectiveTransformMatrix:
"""
Test perspective transform creation.
"""

def test_basic_above_origin_pointed_down(
self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation
) -> None:
"""
Above origin, directly down.
"""
# Setup
result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation(
0.0,
-np.pi / 2,
0.0,
)
assert result
assert drone_rotation_matrix is not None

drone_position_ned = np.array([0.0, 0.0, -100.0], dtype=np.float32)

vec_ground_expected = np.array([-100.0, 100.0, 1.0], dtype=np.float32)

# Run
(
result,
actual,
) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore
drone_rotation_matrix,
drone_position_ned,
)

# Test
assert result
assert actual is not None

vec_ground = actual @ np.array([2000, 2000, 1])
vec_ground_normalized = vec_ground / vec_ground[2]
np.testing.assert_almost_equal(
vec_ground_normalized,
vec_ground_expected,
decimal=FLOAT_PRECISION_TOLERANCE,
)

def test_intermediate_above_origin_pointing_north(
self, intermediate_locator: rudimentary_geolocation.RudimentaryGeolocation
) -> None:
"""
Positioned so that the camera is above the origin directly down (but the drone is not).
"""
# Setup
result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation(
0.0,
0.0,
0.0,
)
assert result
assert drone_rotation_matrix is not None

drone_position_ned = np.array([-1.0, 0.0, -100.0], dtype=np.float32)

vec_ground_expected = np.array([-100.0, 100.0, 1.0], dtype=np.float32)

# Run
(
result,
actual,
) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore
drone_rotation_matrix,
drone_position_ned,
)

# Test
assert result
assert actual is not None

vec_ground = actual @ np.array([2000, 2000, 1])
vec_ground_normalized = vec_ground / vec_ground[2]
np.testing.assert_almost_equal(
vec_ground_normalized,
vec_ground_expected,
decimal=FLOAT_PRECISION_TOLERANCE,
)

def test_intermediate_above_origin_pointing_west(
self, intermediate_locator: rudimentary_geolocation.RudimentaryGeolocation
) -> None:
"""
Positioned so that the camera is above the origin directly down (but the drone is not).
"""
# Setup
result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation(
-np.pi / 2,
0.0,
0.0,
)
assert result
assert drone_rotation_matrix is not None

drone_position_ned = np.array([0.0, 1.0, -100.0], dtype=np.float32)

vec_ground_expected = np.array([100.0, 100.0, 1.0], dtype=np.float32)

# Run
(
result,
actual,
) = intermediate_locator._Geolocation__get_perspective_transform_matrix( # type: ignore
drone_rotation_matrix,
drone_position_ned,
)

# Test
assert result
assert actual is not None

vec_ground = actual @ np.array([2000, 2000, 1])
vec_ground_normalized = vec_ground / vec_ground[2]
np.testing.assert_almost_equal(
vec_ground_normalized,
vec_ground_expected,
decimal=FLOAT_PRECISION_TOLERANCE,
)

def test_advanced(
self, advanced_locator: rudimentary_geolocation.RudimentaryGeolocation
) -> None:
"""
Camera is north of origin with an angle from vertical. Also rotated.
"""
# Setup
result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation(
0.0,
np.pi / 12,
-np.pi,
)
assert result
assert drone_rotation_matrix is not None

drone_position_ned = np.array(
[
10.0 - np.cos(-np.pi / 12), # Camera at 10 units forward
0.0,
-100.0 - np.sin(-np.pi / 12), # Camera at 100 units above ground
],
dtype=np.float32,
)

vec_ground_sanity_expected = np.array([10.0, 0.0, 1.0], dtype=np.float32)
vec_ground_expected = np.array(
[
10.0 + 100.0 * np.sqrt(3),
100.0,
1.0,
],
dtype=np.float32,
)

# Run
(
result,
actual,
) = advanced_locator._Geolocation__get_perspective_transform_matrix( # type: ignore
drone_rotation_matrix,
drone_position_ned,
)

# Test
assert result
assert actual is not None

vec_ground_sanity = actual @ np.array([0, 1000, 1])
vec_ground_sanity_actual = vec_ground_sanity / vec_ground_sanity[2]
np.testing.assert_almost_equal(
vec_ground_sanity_actual,
vec_ground_sanity_expected,
decimal=FLOAT_PRECISION_TOLERANCE,
)

vec_ground = actual @ np.array([2000, 2000, 1])
vec_ground_normalized = vec_ground / vec_ground[2]
np.testing.assert_almost_equal(
vec_ground_normalized,
vec_ground_expected,
decimal=FLOAT_PRECISION_TOLERANCE,
)

def test_bad_direction(
self, basic_locator: rudimentary_geolocation.RudimentaryGeolocation
) -> None:
"""
Camera pointing forward.
"""
# Setup
result, drone_rotation_matrix = camera_properties.create_rotation_matrix_from_orientation(
0.0,
0.0,
0.0,
)
assert result
assert drone_rotation_matrix is not None

drone_position_ned = np.array(
[
0.0,
0.0,
-100.0,
],
dtype=np.float32,
)

# Run
(
result,
actual,
) = basic_locator._Geolocation__get_perspective_transform_matrix( # type: ignore
drone_rotation_matrix,
drone_position_ned,
)

# Test
assert not result
assert actual is None


class TestRudimentaryGeolocationConvertDetection:
"""
Test extract and convert.
"""

def test_normal1(
self, detection_1: detections_and_time.Detection, affine_matrix: np.ndarray
) -> None:
"""
Normal detection and matrix.
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

result, expected = detection_in_world.DetectionInWorld.create(
# fmt: off
np.array(
[
[ -1.0, -1.0],
[ -1.0, 3999.0],
[1999.0, -1.0],
[1999.0, 3999.0],
],
dtype=np.float32,
),
# fmt: on
np.array(
[999.0, 1999.0],
dtype=np.float32,
),
1,
1.0,
)
assert result
assert expected is not None

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore
detection_1,
affine_matrix,
test_logger,
)

# Test
assert result
assert actual is not None

np.testing.assert_almost_equal(actual.vertices, expected.vertices)
np.testing.assert_almost_equal(actual.centre, expected.centre)
assert actual.label == expected.label
np.testing.assert_almost_equal(actual.confidence, expected.confidence)

def test_normal2(
self, detection_2: detections_and_time.Detection, affine_matrix: np.ndarray
) -> None:
"""
Normal detection and matrix.
"""
# Setup
result, test_logger = logger.Logger.create("test_logger", False)
assert result
assert test_logger is not None

result, expected = detection_in_world.DetectionInWorld.create(
# fmt: off
np.array(
[
[ -1.0, -1.0],
[ -1.0, 1999.0],
[999.0, -1.0],
[999.0, 1999.0],
],
dtype=np.float32,
),
# fmt: on
np.array(
[499.0, 999.0],
dtype=np.float32,
),
2,
0.5,
)
assert result
assert expected is not None

# Run
(
result,
actual,
) = rudimentary_geolocation.RudimentaryGeolocation._Geolocation__convert_detection_to_world_from_image( # type: ignore
detection_2,
affine_matrix,
test_logger,
)

# Test
assert result
assert actual is not None

np.testing.assert_almost_equal(actual.vertices, expected.vertices)
np.testing.assert_almost_equal(actual.centre, expected.centre)
assert actual.label == expected.label
np.testing.assert_almost_equal(actual.confidence, expected.confidence)


class TestRudimentaryGeolocationRun:
"""
Run.
@@ -918,3 +202,49 @@ def test_basic(
np.testing.assert_almost_equal(actual.centre, expected_list[i].centre)
assert actual.label == expected_list[i].label
np.testing.assert_almost_equal(actual.confidence, expected_list[i].confidence)

def test_bad_direction(
self,
basic_locator: rudimentary_geolocation.RudimentaryGeolocation,
detection_1: detections_and_time.Detection,
) -> None:
"""
Bad direction.
"""
# Setup
result, drone_position = position_local.PositionLocal.create(
0.0,
0.0,
-100.0,
)
assert result
assert drone_position is not None

result, drone_orientation = orientation.Orientation.create(
0.0,
0.0,
0.0,
)
assert result
assert drone_orientation is not None

result, drone_odometry = drone_odometry_local.DroneOdometryLocal.create(
drone_position,
drone_orientation,
)
assert result
assert drone_odometry is not None

result, merged_detections = merged_odometry_detections.MergedOdometryDetections.create(
drone_odometry,
[detection_1],
)
assert result
assert merged_detections is not None

# Run
result, actual_list = basic_locator.run(merged_detections)

# Test
assert not result
assert actual_list is None