def test_cubic(self):
# SETUP
# Make it a cubic predictor
self.polynomial_predictor = PolynomialPredictor(num_states_to_track=5, poly_degree=3)
# Feed in states that match a parabola going through (-1, 1), (0, 0), (1, 1), (2, 1), (3, 1)
self.polynomial_predictor.add_person_state(PersonState(-1, 1))
time.sleep(1)
self.polynomial_predictor.add_person_state(origin_person_state)
time.sleep(1)
self.polynomial_predictor.add_person_state(PersonState(1, 1))
time.sleep(1)
self.polynomial_predictor.add_person_state(PersonState(2, 1))
time.sleep(1)
self.polynomial_predictor.add_person_state(PersonState(3, 1))
# EXECUTE
predicted_states = self.polynomial_predictor.predict_next_person_state(second_intervals)
# VERIFY
assert len(predicted_states) == len(second_intervals)
prev_x = 1
prev_y = 1
for predicted_state in predicted_states:
# Very loose. Fix me?
assert predicted_state.x > prev_x
assert predicted_state.y < prev_y
prev_x = predicted_state.x
prev_y = predicted_state.y
# Plot things for visual inspection
self.polynomial_predictor.plot_projections(predicted_states)
def test_three_states_given(self):
# SETUP
# Feed in states that match a parabola going through (-1, 1), (0, 0) and (1, 1)
self.polynomial_predictor.add_person_state(PersonState(-1, 1))
time.sleep(1)
self.polynomial_predictor.add_person_state(origin_person_state)
time.sleep(1)
self.polynomial_predictor.add_person_state(PersonState(1, 1))
# EXECUTE
predicted_states = self.polynomial_predictor.predict_next_person_state(second_intervals)
# VERIFY
# Given states provided, should predict somewhere along the parabola.
assert len(predicted_states) == len(second_intervals)
prev_x = 1
prev_y = 1
for predicted_state in predicted_states:
# Very loose. Fix me?
assert predicted_state.x > prev_x
assert predicted_state.y > prev_y
prev_x = predicted_state.x
prev_y = predicted_state.y
# Plot things for visual inspection
self.polynomial_predictor.plot_projections(predicted_states)
def test_cubic_periodic_extrapolation(self):
# SETUP
self.spline_predictor = SplinePredictor(bc_type='natural', extrapolation_type='periodic')
# Feed in states going through (-1, 1), (0, 0), (1, 1), (2, 1), (3, 1)
self.spline_predictor.add_person_state(PersonState(-1, 1))
time.sleep(1)
self.spline_predictor.add_person_state(origin_person_state)
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(1, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(2, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(3, 1))
# EXECUTE
predicted_states = self.spline_predictor.predict_next_person_state(second_intervals)
# VERIFY
assert len(predicted_states) == len(second_intervals)
prev_x = 1
for predicted_state in predicted_states:
# Very loose. Fix me?
assert predicted_state.x > prev_x
prev_x = predicted_state.x
# Plot things for visual inspection
self.spline_predictor.plot_projections(predicted_states)
def test_cubic_standard(self):
# SETUP
# All default (not-a-knot and normal extrapolation based on last interval)
self.spline_predictor = SplinePredictor()
# Feed in states going through (-1, 1), (0, 0), (1, 1), (2, 1), (3, 1)
self.spline_predictor.add_person_state(PersonState(-1, 1))
time.sleep(1)
self.spline_predictor.add_person_state(origin_person_state)
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(1, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(2, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(3, 1))
# EXECUTE
predicted_states = self.spline_predictor.predict_next_person_state(second_intervals)
# VERIFY
assert len(predicted_states) == len(second_intervals)
prev_x = 1
for predicted_state in predicted_states:
# Very loose. Fix me?
assert predicted_state.x > prev_x
prev_x = predicted_state.x
# Plot things for visual inspection
self.spline_predictor.plot_projections(predicted_states)
def predict_next_person_state(self, time_deltas_to_predict):
# Translate time_deltas into global times
current_time = time.time()
times_to_predict = [
time_delta + current_time for time_delta in time_deltas_to_predict
]
num_datapoints = len(self.person_states)
if num_datapoints == 0:
print("ERROR: cannot predict if not given any past data")
return None
if num_datapoints == 1:
print(
"WARNING: asked to predict but only given a single past datapoint."
)
return [self.person_states[0][0] for _ in times_to_predict]
oldest_state, oldest_time = self.person_states[0]
newest_state, newest_time = self.person_states[-1]
delta_x_per_sec = (newest_state.x - oldest_state.x) / (newest_time -
oldest_time)
delta_y_per_sec = (newest_state.y - oldest_state.y) / (newest_time -
oldest_time)
# Project for each of the requested times
projections = []
for time_to_predict in times_to_predict:
x = newest_state.x + delta_x_per_sec * (time_to_predict -
newest_time)
y = newest_state.y + delta_y_per_sec * (time_to_predict -
newest_time)
projections.append(PersonState(x, y))
return projections
def test_waypoint_visualization_with_ngon_generator(self):
# initialize the drone state
waypoint_generator = NGonWaypointGenerator(n=6, radius=4)
gamma = 0.9
cinematic_controller = CinematicController(
waypoint_generator=waypoint_generator, bb_filter_gamma=gamma)
cinematic_controller.update_latest_drone_state(origin_drone_state)
cinematic_controller.update_latest_bbs([bb_10_10_0_0])
# initialize the drone waypoints
waypoints = cinematic_controller.generate_waypoints()
# initialize the person state
person_state = PersonState(4, 0)
# initialize the environment
environment = Environment()
environment.add_obstacles([[(6, 0), (4, 2), (6, 2)],
[(7, -4), (8, -3), (9, -3), (9, -5),
(8, -3.5)], [(-5, -2), (0, -5), (0, -2)]])
# initialize visualization object and plot
viz = WaypointVisualization(waypoints, [person_state], environment)
viz.plot('plots/test_waypoint_visualization_with_ngon_generator.png')
# check that file was saved properly
assert os.path.isfile(
'plots/test_waypoint_visualization_with_ngon_generator.png')
def test_cubic_many_datapoints(self):
# SETUP
self.spline_predictor = SplinePredictor(num_states_to_track=100, bc_type='natural')
# Feed in tons of states.
self.spline_predictor.add_person_state(PersonState(-1, 1))
time.sleep(1)
self.spline_predictor.add_person_state(origin_person_state)
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(1, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(2, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(3, 1))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(4, 2))
time.sleep(1)
self.spline_predictor.add_person_state(PersonState(5, 1))
# EXECUTE
predicted_states = self.spline_predictor.predict_next_person_state(second_intervals)
# VERIFY
assert len(predicted_states) == len(second_intervals)
prev_x = 1
for predicted_state in predicted_states:
# Very loose. Fix me?
assert predicted_state.x > prev_x
prev_x = predicted_state.x
# Plot things for visual inspection
self.spline_predictor.plot_projections(predicted_states)
def predict_next_person_state(self, time_deltas_to_predict):
num_datapoints = len(self.person_states)
if num_datapoints == 0:
print("ERROR: cannot predict if not given any past data")
return None
if num_datapoints == 1:
print(
"WARNING: asked to predict but only given a single past datapoint."
)
return [self.person_states[0][0] for _ in time_deltas_to_predict]
# Estimate the distance covered so we can back out the velocity
distance_covered = 0
for i in range(len(self.person_states) - 1):
current_state = self.person_states[i][0]
next_state = self.person_states[i + 1][0]
distance_covered += np.sqrt((current_state.x - next_state.x)**2 +
(current_state.y - next_state.y)**2)
oldest_state, oldest_time = self.person_states[0]
newest_state, newest_time = self.person_states[-1]
projected_speed = distance_covered / (newest_time - oldest_time)
# Project for each of the requested times
projections = []
step_size_x = 0.00001 # The smaller the better resolution, but also slower computationally.
for time_delta in time_deltas_to_predict:
distance_to_cover = time_delta * projected_speed
# Inch forward along the curve
previous_x = newest_state.x
previous_y = newest_state.y
projected_x = previous_x + step_size_x
projected_y = self.poly(projected_x)
distance_projected = 0
# Obviously, this is a numerical method that's inexact, and what I'm doing will always overshoot
# instead of finding an unbiased estimate, and it's computationally inefficient because I'm
# stepping forward linearly instead of growing step size exponentially. It's super easy to implement
# and understand, at least.
while distance_projected < distance_to_cover:
projected_x = previous_x + step_size_x
projected_y = self.poly(projected_x)
distance_projected += np.sqrt(step_size_x**2 +
(projected_y - previous_y)**2)
previous_x = projected_x
previous_y = projected_y
print("Overshot distance by:",
distance_projected - distance_to_cover)
projections.append(PersonState(projected_x, projected_y))
return projections
def generate_waypoints(self,
bounding_box,
drone_state,
person_predictor=None):
current_x, current_y, current_z = drone_state.get_position()
current_roll, current_pitch, current_yaw = drone_state.get_attitude()
# Compute the desired center of the n-gon.
# Note how we could easily adapt this code to use the person as the center if we can
# somehow get a person's position from the bounding box.
center_x = current_x + math.cos(current_yaw) * self.radius
center_y = current_y + math.sin(current_yaw) * self.radius
time_to_project = 10
timesteps = [i * time_to_project / self.n for i in range(self.n + 1)]
current_person_state = PersonState(center_x, center_y)
predicted_person_states = [
current_person_state for _ in range(self.n + 1)
]
if person_predictor is not None:
predicted_person_states = person_predictor.predict_next_person_state(
timesteps)
target_waypoints = []
angle_per_segment = 2.0 * math.pi / self.n
for i in range(
1, self.n +
1): # Don't include current state as a starting waypoint
predicted_person_state = predicted_person_states[i]
# new_x = math.cos(angle_per_segment * i + math.pi + current_yaw) * self.radius + center_x
# new_y = math.sin(angle_per_segment * i + math.pi + current_yaw) * self.radius + center_y
new_x = math.cos(angle_per_segment * i + math.pi + current_yaw
) * self.radius + predicted_person_state.x
new_y = math.sin(angle_per_segment * i + math.pi + current_yaw
) * self.radius + predicted_person_state.y
new_theta = current_yaw + i * angle_per_segment # Check if need to crop to be within [-2pi, 2pi]
target_waypoints.append(
DroneState(new_x, new_y, current_z, current_roll,
current_pitch, new_theta, 0, 0, 0, 0, 0,
0)) # All velocities are 0
# target_waypoints.append(drone_state) # end up back where you started.
return target_waypoints
def update_latest_bbs(self, all_bounding_boxes):
if all_bounding_boxes is None or len(all_bounding_boxes) == 0:
print("Error, no bounding boxes provided. This case is not handled.")
print("For now, assume that the bounding box just disappeared, so don't change the tracked"
" bounding box at all. Other valid approaches would be to set the latest value to"
" null but not update teh smoothed bounding box.")
self.latest_bounding_box = None
self.smoothed_bounding_box = None
return
best_match_bb = self.identify_best_bb_match(all_bounding_boxes)
self.latest_bounding_box = best_match_bb
# Set the smoothed bounding box, looking out for initialization
# conditions.
if self.smoothed_bounding_box is None:
self.smoothed_bounding_box = best_match_bb
else:
self.smoothed_bounding_box = self.compute_low_pass_filter_bb()
# Compute a person state from the bounding box
if self.person_predictor is not None:
person_state = PersonState.get_person_state_from_bb(self.latest_drone_state, self.smoothed_bounding_box)
self.person_predictor.add_person_state(person_state)
def test_weights(self):
# SETUP
# Override the weight factor for the weighted polynomial predictor (but keep self.pp unchanged)
weighted_polynomial_predictor = PolynomialPredictor(num_states_to_track=4, poly_degree=2, weight_decay_factor=4)
# Feed in states that match a parabola going through (-1, 1), (0, 0), (1, 1), (2, 0)
self.polynomial_predictor.add_person_state(PersonState(-1, 1))
weighted_polynomial_predictor.add_person_state(PersonState(-1, 1))
time.sleep(1)
self.polynomial_predictor.add_person_state(origin_person_state)
weighted_polynomial_predictor.add_person_state(origin_person_state)
time.sleep(1)
self.polynomial_predictor.add_person_state(PersonState(1, 1))
weighted_polynomial_predictor.add_person_state(PersonState(1, 1))
time.sleep(1)
self.polynomial_predictor.add_person_state(PersonState(2, 0))
weighted_polynomial_predictor.add_person_state(PersonState(2, 0))
# EXECUTE
predicted_states = self.polynomial_predictor.predict_next_person_state(second_intervals)
weighted_predicted_states = weighted_polynomial_predictor.predict_next_person_state(second_intervals)
# VERIFY
assert len(predicted_states) == len(second_intervals)
assert len(weighted_predicted_states) == len(second_intervals)
prev_x = 2
prev_y = 0
weighted_prev_x = 2
weighted_prev_y = 0
for i, predicted_state in enumerate(predicted_states):
weighted_predicted_state = weighted_predicted_states[i]
# Very loose. Fix me?
assert predicted_state.x > prev_x
assert predicted_state.y < prev_y
prev_x = predicted_state.x
prev_y = predicted_state.y
assert weighted_predicted_state.x > weighted_prev_x
assert weighted_predicted_state.y < weighted_prev_y
weighted_prev_x = weighted_predicted_state.x
weighted_prev_y = weighted_predicted_state.y
# Plot things for visual inspection
self.polynomial_predictor.plot_projections(predicted_states)
weighted_polynomial_predictor.plot_projections(weighted_predicted_states)
import time
import unittest
from person_detection.person_predictor.linear_predictor import LinearPredictor
from utils.person_state import PersonState
# Define a few helpful variables common across a few tests
origin_person_state = PersonState(0, 0)
x10_person_state = PersonState(10, 0)
ten_second_intervals = [10, 20, 30, 40, 50, 60]
class TestLinearPredictor(unittest.TestCase):
# Before every test, reset the LinearPredictor object that will be tested.
def setUp(self):
self.linear_predictor = LinearPredictor()
def test_one_state_given(self):
# SETUP
self.linear_predictor.add_person_state(origin_person_state)
# EXECUTE
predicted_states = self.linear_predictor.predict_next_person_state(
ten_second_intervals)
# VERIFY
# Given only a single state, the best prediction over any time horizon is to stay in the same place.
assert len(predicted_states) == len(ten_second_intervals)
for predicted_state in predicted_states:
assert predicted_state == origin_person_state
def test_two_states_given(self):
import time
import unittest
from person_detection.person_predictor.polynomial_predictor import PolynomialPredictor
from utils.person_state import PersonState
# Define a few helpful variables common across a few tests
origin_person_state = PersonState(0, 0)
second_intervals = [1, 2, 3, 4, 5, 6]
class TestPolynomialPredictor(unittest.TestCase):
# Before every test, reset the PolynomialPredictor object that will be tested.
def setUp(self):
self.polynomial_predictor = PolynomialPredictor()
def test_one_state_given(self):
# SETUP
self.polynomial_predictor.add_person_state(origin_person_state)
# EXECUTE
predicted_states = self.polynomial_predictor.predict_next_person_state(second_intervals)
# VERIFY
# Given only a single state, the best prediction over any time horizon is to stay in the same place.
assert len(predicted_states) == len(second_intervals)
for predicted_state in predicted_states:
assert predicted_state == origin_person_state
def test_three_states_given(self):
# SETUP