Removed the subdir.

pyrate/tests/sense/filters/test_kalman.py

@@ -0,0 +1,194 @@
+"""This module asserts correct runtime behaviour of the pyrate.sense.filter subpackage."""
+
+# Python standard library
+from datetime import timedelta
+from typing import cast
+
+# Test environment
+from unittest import TestCase
+
+# Mathematics
+from numpy import array
+from numpy import eye
+from numpy import Inf
+from numpy.linalg import norm
+from numpy import ndarray
+from numpy import vstack
+
+# Hypothesis testing
+from hypothesis import given
+from hypothesis import settings
+
+# Package under test
+from pyrate.common.math import Gaussian
+from pyrate.sense.filters import ExtendedKalman
+from pyrate.sense.filters import Kalman
+from pyrate.sense.filters import UnscentedKalman
+
+# Helpers
+from pyrate.common.testing.strategies.dynamic_system import linear_model
+from pyrate.common.testing.strategies.dynamic_system import nonlinear_model
+
+# Flags from Pyrate
+from pyrate.common.testing import IS_EXTENDED_TESTING
+
+
+class TestKalman(TestCase):
+
+    """Test for correct runtime behaviour of Kalman filters pyrate.sense.filter."""
+
+    # In this context, we reproduce a common filter notation
+    # pylint: disable=invalid-name
+
+    @given(linear_model())
+    @settings(max_examples=1000 if IS_EXTENDED_TESTING else 10, deadline=timedelta(seconds=1.0))
+    def test_kalman(self, model):
+        """Assert the correct functionality of the standard Kalman filter."""
+
+        # Unpack generated model
+        estimate, F, B, H, Q, R, measurements, inputs = model
+
+        # Initialize filter
+        kalman = Kalman(F=F, estimate=estimate, H=H, Q=Q, R=R, B=B, keep_trace=True)
+
+        # Apply the Kalman filter a few times
+        for i, measurement in enumerate(measurements):
+            kalman.predict(u=inputs[i])
+            kalman.correct(z=measurement)
+
+        # Assert correct tracing with Kalman
+        self.assertIsNotNone(kalman.predictions, "Kalman filter did not keep trace of predictions")
+        self.assertIsNotNone(kalman.estimates, "Kalman filter did not keep trace of estimates")
+        self.assertEqual(
+            len(kalman.predictions.index), len(measurements), "Kalman filter has not traced all predictions"
+        )
+        self.assertEqual(
+            len(kalman.estimates.index), len(measurements), "Kalman filter has not traced all estimates"
+        )
+
+        # Apply the Kalman filter with a callable H
+        callable_H = lambda dummy: H  # noqa: E731
+        kalman.H = callable_H
+        kalman.correct(z=measurements[0], dummy=None)
+
+    @given(nonlinear_model())
+    @settings(max_examples=1000 if IS_EXTENDED_TESTING else 10, deadline=timedelta(seconds=1.0))
+    def test_extended_kalman(self, model):
+        """Assert the correct functionality of the extended Kalman filter."""
+
+        # Unpack generated model
+        estimate, f, _, Jf, h, _, Jh, Q, R, measurements = model
+
+        # Initialize filter
+        extended = ExtendedKalman(F=Jf, f=f, estimate=estimate, H=Jh, h=h, Q=Q, R=R, keep_trace=True)
+
+        # Apply the Kalman filter a few times
+        for z in measurements:
+            extended.predict()
+            extended.correct(z)
+
+        # Assert correct tracing with Kalman
+        self.assertIsNotNone(extended.predictions, "Extended Kalman filter did not keep trace of predictions")
+        self.assertIsNotNone(extended.estimates, "Extended Kalman filter did not keep trace of estimates")
+        self.assertEqual(
+            len(extended.predictions.index),
+            len(measurements),
+            "Extended Kalman filter has not traced all predictions",
+        )
+        self.assertEqual(
+            len(extended.estimates.index),
+            len(measurements),
+            "Extended Kalman filter has not traced all estimates",
+        )
+
+    @given(nonlinear_model())
+    @settings(max_examples=1000 if IS_EXTENDED_TESTING else 10, deadline=timedelta(seconds=1.0))
+    def test_unscented_kalman(self, model):
+        """Assert the correct functionality of the extended Kalman filter."""
+
+        # Unpack generated model
+        estimate, f, _, _, h, _, _, Q, R, measurements = model
+
+        # Initialize filter
+        unscented = UnscentedKalman(f=f, estimate=estimate, h=h, Q=Q, R=R, keep_trace=True)
+
+        # Apply the Kalman filter a few times
+        for z in measurements:
+            unscented.predict()
+            unscented.correct(z)
+
+        # Assert correct tracing with Kalman
+        self.assertIsNotNone(
+            unscented.predictions, "Uncented Kalman filter did not keep trace of predictions"
+        )
+        self.assertIsNotNone(unscented.estimates, "Uncented Kalman filter did not keep trace of estimates")
+        self.assertEqual(
+            len(unscented.predictions.index),
+            len(measurements),
+            "Uncented Kalman filter has not traced all predictions",
+        )
+        self.assertEqual(
+            len(unscented.estimates.index),
+            len(measurements),
+            "Uncented Kalman filter has not traced all estimates",
+        )
+
+    @staticmethod
+    def test_estimation():
+        """Assert that the filter estimates tend towards the true state over time."""
+
+        # Define model of a constant value
+        F = H = Q = R = eye(1)
+
+        # Initial belief
+        estimate = Gaussian(vstack([0.0]), eye(1))
+
+        # Initialize filters
+        kalman = Kalman(F=F, estimate=estimate, H=H, Q=Q, R=R)
+        extended = ExtendedKalman(
+            F=lambda _: F,
+            f=lambda x: cast(ndarray, x),
+            estimate=estimate,
+            H=lambda _: H,
+            h=lambda x: cast(ndarray, x),
+            Q=Q,
+            R=R,
+        )
+        unscented = UnscentedKalman(
+            f=lambda x: cast(ndarray, x),
+            estimate=estimate,
+            h=lambda x: cast(ndarray, x),
+            Q=Q,
+            R=R,
+        )
+
+        # Apply the Kalman filter a few times
+        true_state = array([20.0])
+        previous_kalman_error = Inf
+        previous_extended_error = Inf
+        previous_unscented_error = Inf
+        for _ in range(10):
+            # Check error going down for Kalman
+            kalman.predict()
+            kalman.correct(z=true_state)
+            error = norm(kalman.estimate.x - true_state).item()  # Convert from numpy scalar to Python float
+            assert (
+                error < previous_kalman_error or error == 0
+            ), "Kalman estimate did not get better over time."
+            previous_kalman_error = error
+
+            # Check error going down for EKF
+            extended.predict()
+            extended.correct(z=true_state)
+            error = norm(extended.estimate.x - true_state).item()
+            assert error < previous_extended_error or error == 0, "EKF estimate did not get better over time."
+            previous_extended_error = error
+
+            # Check error going down for UKF
+            unscented.predict()
+            unscented.correct(z=true_state)
+            error = norm(unscented.estimate.x - true_state).item()
+            assert (
+                error < previous_unscented_error or error == 0
+            ), "UKF estimate did not get better over time."
+            previous_unscented_error = error

pyrate/tests/sense/filters/test_phd.py

@@ -0,0 +1,145 @@
+"""This module asserts correct runtime behaviour of the pyrate.sense.filter subpackage."""
+
+# Python standard library
+from datetime import timedelta
+
+# Test environment
+from unittest import TestCase
+
+# Hypothesis testing
+from hypothesis import given
+from hypothesis import settings
+
+# Package under test
+from pyrate.sense.filters import ExtendedGaussianMixturePHD
+from pyrate.sense.filters import GaussianMixturePHD
+
+# Helpers
+from pyrate.common.testing.strategies.dynamic_system import linear_model
+from pyrate.common.testing.strategies.dynamic_system import nonlinear_model
+
+# Flags from Pyrate
+from pyrate.common.testing import IS_EXTENDED_TESTING
+
+
+class TestPHD(TestCase):
+
+    """Test for correct runtime behaviour in pyrate.sense.filter."""
+
+    # In this context, we reproduce a common filter notation
+    # pylint: disable=invalid-name
+
+    def setUp(self) -> None:
+        """Setup the linear motion model for the filter tests."""
+
+        # Survival and detection rate of targets
+        self.survival_rate = 0.99
+        self.detection_rate = 0.99
+
+        # Clutter intensity
+        self.intensity = 0.01
+
+        # PHD pruning
+        self.threshold = 0.1
+        self.merge_distance = 0.5
+        self.max_components = 0.5
+
+    @given(linear_model())
+    @settings(max_examples=1000 if IS_EXTENDED_TESTING else 10, deadline=timedelta(seconds=1.0))
+    def test_gmphd(self, model):
+        """Assert the correct functionality of the gaussian mixture PHD filter."""
+
+        # Unpack generated model
+        estimate, F, _, H, Q, R, measurements, _ = model
+
+        # Initialize filter
+        gmphd = GaussianMixturePHD(
+            birth_belief=[estimate],
+            survival_rate=self.survival_rate,
+            detection_rate=self.detection_rate,
+            intensity=self.intensity,
+            F=F,
+            H=H,
+            Q=Q,
+            R=R,
+        )
+
+        self.assertEqual(len(gmphd.gmm), 0, "Mixture model should be initialy empty")
+
+        # Predict with a callable F and check number of components
+        callable_F = lambda dummy: F  # noqa: E731
+        gmphd.F = callable_F
+        gmphd.predict(dummy=None)
+        gmphd.F = F
+        self.assertEqual(
+            len(gmphd.gmm), len(gmphd.birth_belief), "Mixture model is not the right size after prediction"
+        )
+
+        # Apply the PHD filter with a callable H and check number of components
+        callable_H = lambda dummy: H  # noqa: E731
+        gmphd.H = callable_H
+        gmphd.correct(measurements=measurements, dummy=None)
+        gmphd.H = H
+        self.assertEqual(
+            len(gmphd.gmm), len(measurements) + 1, "Mixture model is not the right size after correction"
+        )
+
+        # Removing all components
+        gmphd.prune(self.threshold, self.merge_distance, 0)
+        self.assertEqual(len(gmphd.extract()), 0, "Pruning did not remove all components")
+
+        # Apply the filter multiple times
+        for _, _ in enumerate(measurements):
+            gmphd.predict()
+            gmphd.correct(measurements=measurements)
+
+        # Extract states
+        gmphd.extract(self.threshold)
+
+    @given(nonlinear_model())
+    @settings(max_examples=1000 if IS_EXTENDED_TESTING else 10, deadline=timedelta(seconds=1.0))
+    def test_extended_gmphd(self, model):
+        """Assert the correct functionality of the gaussian mixture PHD filter."""
+
+        # Unpack generated model
+        estimate, f, _, Jf, h, _, Jh, Q, R, measurements = model
+
+        # Initialize filter
+        gmphd = ExtendedGaussianMixturePHD(
+            birth_belief=[estimate],
+            survival_rate=self.survival_rate,
+            detection_rate=self.detection_rate,
+            intensity=self.intensity,
+            F=Jf,
+            f=f,
+            H=Jh,
+            h=h,
+            Q=Q,
+            R=R,
+        )
+
+        self.assertEqual(len(gmphd.gmm), 0, "Mixture model should be initialy empty")
+
+        # Predict and check number of components
+        gmphd.predict()
+        self.assertEqual(
+            len(gmphd.gmm), len(gmphd.birth_belief), "Mixture model is not the right size after prediction"
+        )
+
+        # Apply the PHD filter and check number of components
+        gmphd.correct(measurements=measurements)
+        self.assertEqual(
+            len(gmphd.gmm), len(measurements) + 1, "Mixture model is not the right size after correction"
+        )
+
+        # Removing all components
+        gmphd.prune(self.threshold, self.merge_distance, 0)
+        self.assertEqual(len(gmphd.extract()), 0, "Pruning did not remove all components")
+
+        # Apply the filter multiple times
+        for _, _ in enumerate(measurements):
+            gmphd.predict()
+            gmphd.correct(measurements=measurements)
+
+        # Extract states
+        gmphd.extract(self.threshold)

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_1.txt

@@ -0,0 +1,3 @@
+19,237
+432,242
+0.69

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_10.txt

@@ -0,0 +1,3 @@
+17,262
+630,273
+1.03

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_13.txt

@@ -0,0 +1,3 @@
+14,277
+389,279
+0.31

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_16.txt

@@ -0,0 +1,3 @@
+153,286
+638,291
+0.59

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_18.txt

@@ -0,0 +1,3 @@
+160,286
+623,290
+0.49

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_19.txt

@@ -0,0 +1,3 @@
+48,271
+411,273
+0.32

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_3.txt

@@ -0,0 +1,3 @@
+21,253
+598,261
+0.79

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_4.txt

@@ -0,0 +1,3 @@
+67,266
+415,274
+1.32

pyrate/tests/sense/vision/resources/testing_dataset_successful/annotations/Preprocessed_test_7.txt

@@ -0,0 +1,3 @@
+7,257
+242,262
+1.22

pyrate/tests/sense/vision/test_image_line.py

@@ -0,0 +1,61 @@
+"""This test suite runs additional tests for ``ImageLine`` that are not covered in ``TestObstacleLocator``"""
+
+# Testing
+from unittest import TestCase
+
+# Hypothesis
+from hypothesis import given
+from hypothesis.strategies import composite
+from hypothesis.strategies import floats
+from hypothesis.strategies import integers
+from hypothesis.strategies import just
+from hypothesis.strategies import tuples
+
+# Scientific
+from numpy import pi
+
+# Module under test
+from pyrate.sense.vision.image_line import ImageLine
+
+
+@composite
+def image_dimensions_and_points(draw):
+    """Generate image dimensions and points left and right on that image"""
+
+    image_dims = draw(tuples(integers(1, 10000), integers(1, 10000)))
+    point_a = draw(tuples(just(0), integers(0, image_dims[1] - 1)))
+    point_b = draw(tuples(just(image_dims[0] - 1), integers(0, image_dims[1] - 1)))
+
+    return image_dims, point_a, point_b
+
+
+class TestImageLine(TestCase):
+
+    """Tests the remaining methods of ``ImageLine`` not covered by testing ``ObstacleLocator``"""
+
+    @given(floats(1, 10000), floats(1, 10000), floats(-5000, 5000), floats(0, 2 * pi))
+    def test_from_height_angle(self, image_width, image_height, height, angle):
+        """Test that creates (from height and angle) and tests ``ImageLine``s"""
+
+        image_line = ImageLine.from_height_angle((image_width, image_height), height, angle)
+
+        self.assertTrue(image_line.image_width == image_width and image_line.image_height == image_height)
+        self.assertTrue(image_line.angle == angle)
+        self.assertAlmostEqual(image_line.height, int(height + image_height / 2))
+
+        end_points = image_line.end_points
+        self.assertTrue(
+            end_points[0][0] == 0 and end_points[1][0] == image_width,
+            msg=f"x1={end_points[0][0]} x2={end_points[1][0]}",
+        )
+
+    @given(test_input=image_dimensions_and_points())
+    def test_indices(self, test_input):
+        """Test that tests the ``indices`` property of ``ImageLine``"""
+
+        image_dims, point1, point2 = test_input
+
+        image_line = ImageLine.from_points(image_dims, (point1, point2))
+        x_coords, y_coords = image_line.indices
+        self.assertTrue(((50 <= x_coords) <= 50).all())
+        self.assertTrue(((0 <= y_coords) <= 200).all())

pyrate/tests/sense/vision/test_image_rectangle.py

@@ -0,0 +1,102 @@
+"""This test suite evaluates and tests behavior of the ``ImageRectangle`` class"""
+
+# Testing
+from unittest import TestCase
+
+# Hypothesis
+from hypothesis import given
+from hypothesis.strategies import integers
+
+# Module under test
+from pyrate.sense.vision.image_rectangle import ImageRectangle
+
+
+class TestImageRectangle(TestCase):
+
+    """Tests functionality of the ``ImageRectangle`` class"""
+
+    @given(
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+    )
+    # pylint: disable=too-many-arguments
+    def test_bottom_center(self, position_x, position_y, width, height, offset_x, offset_y):
+        """Parametrized test that tests correct functionality of the bottom_center property
+
+        Args:
+            position_x: x position of the rectangle
+            position_y: y position of the rectangle
+            width: width of the rectangle
+            height: height of the rectangle
+            offset_x: x component of the offset
+            offset_y: y component of the offset
+        """
+
+        offset = (offset_x, offset_y)
+
+        rectangle_without_offset = ImageRectangle((position_x, position_y, width, height))
+        self.assertTupleEqual(rectangle_without_offset.offset, (0, 0))
+        self.assertAlmostEqual(rectangle_without_offset.bottom_center[0], position_x + (width / 2), delta=0.5)
+        self.assertAlmostEqual(rectangle_without_offset.bottom_center[1], position_y + height)
+
+        rectangle_with_offset = ImageRectangle((position_x, position_y, width, height), offset=offset)
+        self.assertTupleEqual(rectangle_with_offset.offset, offset)
+        self.assertAlmostEqual(
+            rectangle_with_offset.bottom_center[0], position_x + offset_x + (width / 2), delta=0.5
+        )
+        self.assertAlmostEqual(rectangle_with_offset.bottom_center[1], position_y + offset_y + height)
+
+    @given(
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+        integers(0, 10000),
+    )
+    # pylint: disable=too-many-arguments
+    def test_rectangle_to_corner(self, position_x, position_y, width, height, offset_x, offset_y):
+        """Parametrized test that tests correct functionality of the rectangle_to_corner method
+
+        Args:
+            position_x: x position of the rectangle
+            position_y: y position of the rectangle
+            width: width of the rectangle
+            height: height of the rectangle
+            offset_x: x component of the offset
+            offset_y: y component of the offset
+        """
+
+        offset = (offset_x, offset_y)
+
+        # rectangle without offset
+        rectangle_without_offset = ImageRectangle((position_x, position_y, width, height))
+        self.assertTupleEqual(rectangle_without_offset.offset, (0, 0))
+        cornerlu, cornerrb = rectangle_without_offset.rectangle_to_corner(offset=False)
+
+        self.assertTrue(
+            cornerlu[0] == position_x and cornerlu[1] == position_y,
+            msg=f"Left upper corner: {cornerlu}",
+        )
+        self.assertTrue(
+            cornerrb[0] == position_x + width and cornerrb[1] == position_y + height,
+            msg=f"Right bottom corner: {cornerrb}",
+        )
+
+        # rectangle with offset
+        rectangle_with_offset = ImageRectangle((position_x, position_y, width, height), offset)
+        self.assertTupleEqual(rectangle_with_offset.offset, offset)
+        cornerlu, cornerrb = rectangle_with_offset.rectangle_to_corner(offset=True)
+
+        self.assertTrue(
+            cornerlu[0] == position_x + offset_x and cornerlu[1] == position_y + offset_y,
+            msg=f"Left upper corner: {cornerlu}",
+        )
+        self.assertTrue(
+            cornerrb[0] == position_x + offset_x + width and cornerrb[1] == position_y + offset_y + height,
+            msg=f"Right bottom corner: {cornerrb}",
+        )

pyrate/tests/sense/vision/test_obstacle_locator.py

@@ -0,0 +1,103 @@
+"""This test suite evaluates and tests behavior of the ``ObstacleLocator`` class"""
+
+# Standard library
+from csv import reader
+from math import radians
+from pathlib import Path
+
+# Typing
+from typing import cast
+from typing import Tuple
+
+# Testing
+from unittest import TestCase
+
+# Scientific
+from cv2 import imread
+
+# Module under test
+from pyrate.sense.vision.image_line import ImageLine
+from pyrate.sense.vision.obstacle_locator import ObstacleLocator
+
+
+PATH_TO_DATASET = Path(__file__).parent / "resources" / "testing_dataset_successful"
+DATASET_IMAGES_PATHS = sorted(list((PATH_TO_DATASET / "testims").glob("*.jpg")))
+DATASET_ANNOTATIONS_PATHS = sorted(list((PATH_TO_DATASET / "annotations").glob("*.txt")))
+
+PATH_TO_FAILING = (
+    Path(__file__).parent / "resources" / "testing_dataset_no_horizon" / "testims" / "Preprocessed_test_0.jpg"
+)
+IMAGE_HEIGHT, IMAGE_WIDTH = imread(PATH_TO_FAILING.as_posix()).shape[:2]
+
+
+class TestObstacleLocator(TestCase):
+
+    """Test for correct predictions made by ``ObstacleLocator``"""
+
+    @staticmethod
+    def parse_annotation(file_path: str, obstacle_locator: ObstacleLocator) -> Tuple[ImageLine, float]:
+        """Helper function to parse the ground truth labels from the dataset.
+
+        Args:
+            file_path: Label file path
+            obstacle_locator: the ObstacleLocator that returns the ImageLine that should be
+            compared to the returned ImageLine of this function
+
+        Returns:
+            ImageLine as described in the annotation, angle read from annotation
+            (for testing correct angle calculation)
+        """
+
+        with open(file_path, "rt", encoding="UTF-8") as label_file:
+            content = label_file.read().split("\n")
+
+        csvreader = reader(content, delimiter=",")
+        point_a = cast(Tuple[int, int], tuple(int(x) for x in next(csvreader)))
+        point_b = cast(Tuple[int, int], tuple(int(x) for x in next(csvreader)))
+        label_angle = radians(float(next(csvreader)[0]))
+
+        line = ImageLine.from_points(
+            image_shape=(obstacle_locator.image_width, obstacle_locator.image_height),
+            points=(point_a, point_b),
+        )
+
+        return line, label_angle
+
+    def test_horizon_angle(self):
+        """Compares ``ObstacleLocator`` horizon estimates to ground truth annotations"""
+
+        uut_ol = ObstacleLocator(image_width=IMAGE_WIDTH, image_height=IMAGE_HEIGHT)  # unit/module under test
+
+        for image_path, label_path in zip(DATASET_IMAGES_PATHS, DATASET_ANNOTATIONS_PATHS):
+            with self.subTest(image=image_path.name):
+                # Assert that we have the correct label for the test image
+                self.assertEqual(
+                    image_path.name.split(".")[0],
+                    label_path.name.split(".")[0],
+                    msg="That isn't the right label for the image. This shouldn't happen.",
+                )
+
+                image = imread(image_path.as_posix())
+
+                # read annotation and test if ImageLine calculates the line's angle correctly
+                label_image_line, label_angle = self.parse_annotation(label_path.as_posix(), uut_ol)
+                self.assertAlmostEqual(label_angle, label_image_line.angle, places=2)
+
+                result = uut_ol.detect_horizon(image)
+                horizons = result[0]
+
+                # Test that a) a horizon is detected and b) it has the correct angle
+                self.assertTrue(len(horizons) > 0, msg="No horizon was detected.")
+                self.assertAlmostEqual(
+                    horizons[0].angle, label_image_line.angle, places=1, msg="Horizon angle mismatch."
+                )
+
+    def test_missing_lines(self):
+        """Tests the branch when no horizon line is detected in the image"""
+
+        uut_ol = ObstacleLocator(image_width=IMAGE_WIDTH, image_height=IMAGE_HEIGHT)  # unit/module under test
+
+        image = imread(PATH_TO_FAILING.as_posix())
+        # ObstacleLocator does not find a horizon line in this image
+        result = uut_ol.detect_horizon(image)
+        self.assertFalse(result[0])