Commit c0ec79dc authored by Brian Hou's avatar Brian Hou
Browse files

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.
parent 78941775
Pipeline #411583 failed with stage
in 5 minutes and 10 seconds
......@@ -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)
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment