Update Project 2: Localization.

Add new autograder tests to catch motion model errors earlier. In particular,
later particle filter tests will fail with a slow unvectorized motion model
implementation.

Update sensor model likelihood plot with a more interpretable default option
that only considers the particle positions (and doesn't encode their angle with
color). The option to color particles by angle is still available with
show_angle=True, and further emphasizes confident particles to make them more
visible.

localization/scripts/make_sensor_model_likelihood_plot

@@ -17,7 +17,7 @@ from cse478 import utils
 from localization.sensor_model import LaserScanSensorModelROS
 
 
-def plot_sensor_model(raw_map_msg, laser_msg, sensor_params=None):
+def plot_sensor_model(raw_map_msg, laser_msg, sensor_params=None, show_angle=False):
     """Visualize the sensor model likelihood as a grayscale image.
 
     Args:
@@ -56,7 +56,7 @@ def plot_sensor_model(raw_map_msg, laser_msg, sensor_params=None):
     utils.map_to_world(particles, map_info)
     weights = np.full((n_particles), 1.0 / float(n_particles))
     rospy.loginfo(
-        "Creating {} particles in {} poses".format(n_particles, n_permissible)
+        "Creating {} particles in {} positions".format(n_particles, n_permissible)
     )
 
     # Instantiate sensor model
@@ -82,19 +82,34 @@ def plot_sensor_model(raw_map_msg, laser_msg, sensor_params=None):
     weights = np.amax(weights, axis=0)
     rospy.logdebug("Weights shape", weights.shape)
 
-    # Rescale weights linearly to [0.1, 0.9]
+    # Rescale weights linearly to [0.0, 1.0]
     w_min = np.amin(weights)
     w_max = np.amax(weights)
     rospy.loginfo("w_min = {:f}".format(w_min))
     rospy.loginfo("w_max = {:f}".format(w_max))
-    weights = 0.9 * (weights - w_min) / (w_max - w_min) + 0.1
+    weights = (weights - w_min) / (w_max - w_min)
 
     # Plot sensor likelihoods as image
-    img = np.zeros((map_msg.info.height, map_msg.info.width, 3))
+    if show_angle:
+        img = np.zeros((map_msg.info.height, map_msg.info.width, 3))
+    else:
+        img = np.zeros((map_msg.info.height, map_msg.info.width))
     for x, y, weight, angle in zip(permissible_x, permissible_y, weights, angles):
-        # Encode angle in hue, as this is a circular dimension
-        # Encode weight in lightness, so less confident particles fade to grey
-        img[x, y] = hsluv_to_rgb((angle * 360.0 / (2 * np.pi), weight * 100.0, 70.0))
+        if show_angle:
+            # Encode angle in hue, as this is a circular dimension.
+            # Encode weight in saturation, so less confident particles fade to grey. Make more confident
+            # particles darker (so the less confident grey is boosted to weight)
+            img[x, y] = hsluv_to_rgb(
+                (
+                    angle * 360.0 / (2 * np.pi),
+                    (0.8 * weight + 0.2) * 100.0,
+                    90 - 30 * weight,
+                )
+            )
+        else:
+            # Just show weight of most probable particle.
+            # Offset from 0 so map shows through
+            img[x, y] = 0.8 * weight + 0.2
 
     img = np.flip(img, axis=0)
     plt.figure(figsize=(map_msg.info.width / 100, map_msg.info.height / 100), dpi=100)
@@ -115,5 +130,6 @@ if __name__ == "__main__":
     laser_msg = rospy.wait_for_message("/car/scan", LaserScan)
     rospy.loginfo("Received map and laser scan message.")
     rospy.loginfo("Preparing the plot, which may take a while.")
-    plot_sensor_model(raw_map_msg, laser_msg, sensor_params)
+    show_angle = rospy.get_param("~show_angle", False)
+    plot_sensor_model(raw_map_msg, laser_msg, sensor_params, show_angle=show_angle)
 

localization/test/motion_model.py

@@ -3,6 +3,7 @@
 import numpy as np
 import rosunit
 import unittest
+import time
 
 from localization.motion_model import KinematicCarMotionModel
 
@@ -49,6 +50,20 @@ class TestMotionModel(unittest.TestCase):
         )
         self.particles = np.zeros((self.num_particles, 3))
 
+    def test_compute_changes_is_vectorized(self):
+        self.num_particles = 1000000
+        self.particles = np.random.uniform(size=(self.num_particles, 3))
+        controls = np.random.uniform([-1, -np.pi], [1, np.pi], (self.num_particles, 2))
+        start = time.time()
+        self.motion_model.compute_changes(self.particles, controls, 0.1)
+        end = time.time()
+        self.assertLess(
+            end - start,
+            5,
+            msg="Your implementation should be able to "
+            "compute changes for one million particles in less than 5 seconds",
+        )
+
     def test_compute_changes_shape(self):
         changes = self.motion_model.compute_changes(
             np.array([[0, 0, 0]]), np.array([[1.0, 0]]), 0.123
@@ -95,6 +110,23 @@ class TestMotionModel(unittest.TestCase):
             err_msg="A linear control's response is linear in time",
         )
 
+    def test_compute_changes_position_invariant(self):
+        controls = np.random.uniform([-1, -np.pi], [1, np.pi], (self.num_particles, 2))
+        t = 0.1
+        changes = self.motion_model.compute_changes(self.particles, controls, t)
+        # Make particles with random coordinates but a 0rad heading
+        self.particles = np.random.uniform(
+            [-10, -10, 0], [10, 10, 0], self.particles.shape
+        )
+        changes_position_randomized = self.motion_model.compute_changes(
+            self.particles, controls, t
+        )
+        np.testing.assert_allclose(
+            changes,
+            changes_position_randomized,
+            err_msg="The input states' position components should not impact the calculation of changes",
+        )
+
     def test_compute_changes_correct_for_large_angular_change(self):
         # Make car length a simple size
         self.motion_model = KinematicCarMotionModel(1.0)
@@ -118,19 +150,59 @@ class TestMotionModel(unittest.TestCase):
             "A non-zero linear velocity and a zero steering velocity should result in linear motion",
         )
 
-    def test_compute_changes_tiny_steering_angle(self):
+    def test_compute_changes_respects_current_heading(self):
+        controls = np.array([[1, 0]])
+        t = 1.0
         changes = self.motion_model.compute_changes(
-            np.array([[0, 0, np.pi / 4]]), np.array([[1.0, 0.0]]), 0.456
+            np.array([[0, 0, np.pi]]), controls, t
         )
-        self.assertEqual(
-            0.456 / np.sqrt(2),
-            changes[0, 1],
-            "A non-zero linear velocity and a near-zero steering velocity should result in linear motion",
+        np.testing.assert_almost_equal(
+            changes[0, :],
+            [-1.0, 0.0, 0.0],
+            err_msg="Linear motion should point in the direction of the current heading",
         )
-        self.assertEqual(
-            0.0,
-            changes[0, 2],
-            "A non-zero linear velocity and a near-zero steering velocity should result in linear motion",
+        changes = self.motion_model.compute_changes(
+            np.array([[0, 0, np.pi / 4]]), np.array([[1.0, 0.0]]), 2.0
+        )
+        # 45-45-90 right triangle with hypotenuse length 2.0
+        np.testing.assert_almost_equal(
+            changes[0, :],
+            [np.sqrt(2), np.sqrt(2), 0],
+            err_msg="Linear motion should point in the direction of the current heading",
+        )
+
+    def test_compute_changes_steering_angle_threshold(self):
+        threshold = 1e-2
+        changes = self.motion_model.compute_changes(
+            np.array([[0, 0, 0], [0, 0, 0]]),
+            np.array([[1.0, 5e-3], [1.0, -5e-3]]),
+            1.0,
+            delta_threshold=threshold,
+        )
+        np.testing.assert_equal(
+            changes[0, :],
+            [1, 0, 0],
+            "A near-zero steering angle should be treated as a zero steering angle, "
+            "with the result being linear motion",
+        )
+        np.testing.assert_equal(
+            changes[1, :],
+            [1, 0, 0],
+            "A negative, near-zero steering angle should be treated as a zero steering angle, "
+            "with the result being linear motion",
+        )
+
+    def test_apply_is_vectorized(self):
+        self.num_particles = 1000000
+        self.particles = np.random.uniform(size=(self.num_particles, 3))
+        start = time.time()
+        self.motion_model.apply_motion_model(self.particles, -1, np.pi / 2, 0.1)
+        end = time.time()
+        self.assertLess(
+            end - start,
+            5,
+            msg="Your implementation should be able to update "
+            "one million particles in less than 5 seconds",
         )
 
     def test_apply_modifies_particles(self):
@@ -154,7 +226,25 @@ class TestMotionModel(unittest.TestCase):
             msg="Motion model should enforce that resulting particle angles are in (-pi, pi]",
         )
 
-    def test_apply_post_control_noise(self):
+        # Check low end is  open
+        self.particles = np.array([[0, 0, -np.pi], [0, 0, -3 * np.pi], [0, 0, np.pi]])
+        self.motion_model.apply_motion_model(self.particles, 0.0, 0.0, 1.0)
+        thetas = self.particles[:, 2]
+        self.assertTrue(
+            np.all((-np.pi < thetas) & (thetas <= np.pi)),
+            msg="Motion model should enforce that resulting particle angles are in (-pi, pi]",
+        )
+
+        # Check top end is closed
+        self.particles = np.array([[0, 0, np.pi], [0, 0, 3 * np.pi]])
+        self.motion_model.apply_motion_model(self.particles, 0.0, 0.0, 1.0)
+        thetas = self.particles[:, 2]
+        self.assertTrue(
+            np.all((-np.pi < thetas) & (thetas <= np.pi)),
+            msg="Motion model should enforce that resulting particle angles are in (-pi, pi]",
+        )
+
+    def test_apply_model_noise(self):
         # We need a lot of particles to get a good estimate of the variance
         self.num_particles = 100000
 
@@ -237,7 +327,7 @@ class TestMotionModel(unittest.TestCase):
             "in the updated headings",
         )
 
-    def test_apply_control_noise(self):
+    def test_apply_action_noise(self):
         # We need a lot of particles to get a good estimate of the variance
         self.num_particles = 100000
 
@@ -288,7 +378,7 @@ class TestMotionModel(unittest.TestCase):
             "changes for a linear control",
         )
 
-    def test_apply_control_noise_impact_grows_with_time(self):
+    def test_apply_action_noise_impact_grows_with_time(self):
         self.num_particles = 100000
         self.particles = np.zeros((self.num_particles, 3))
         delta_std = 0.1