def test_prune_objects(self):
"""Tests prune objects method"""
# create objects to add to map
rng_list = [12.512, 44, 50]
bearing_list = [-22, 81.5, 2]
type_list = [ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY]
update_hist_list = [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1], \
[1, 0, 1, 0, 1, 0, 1, 0, 1, 1], \
[1, 0, 0, 1, 0, 0, 0, 0, 1, 0]]
# loop through objects and add to map
for ii, (rng, bearing,
obj_type) in enumerate(zip(rng_list, bearing_list,
type_list)):
obj = Object(bearing, rng, time_in_millis(), objectType=obj_type)
obj.updateHist = update_hist_list[ii]
self.map.object_list.append(obj)
# create local copy of original object list
orig_object_list = self.map.object_list
# call prune objects
self.map.prune_objects()
# create truth object list
truth_obj_list = orig_object_list[0:2]
# ensure that only the first two objects remain in the object list
self.assertEqual(truth_obj_list, self.map.object_list)
def test_get_buoys(self):
"""Tests get buoys method"""
# create objects to add to map
rng_list = [12.512, 44, 50]
bearing_list = [-22, 81.5, 2]
type_list = [ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY]
# set up correct object list
correct_object_list = [0] * 2
num_correct_objects = 0
# loop through objects and add to map
for rng, bearing, obj_type in zip(rng_list, bearing_list, type_list):
obj = Object(bearing, rng, time_in_millis(), objectType=obj_type)
self.map.object_list.append(obj)
# add object to correct object list if is buoy
if obj_type == ObjectType.BUOY:
correct_object_list[num_correct_objects] = [
rng, bearing, obj_type
]
num_correct_objects += 1
# get list of objects from get_buoys
returned_objects = self.map.get_buoys()
# check if objects map correct object list
for jj, obj in enumerate(correct_object_list):
self.assertAlmostEqual(obj[0], returned_objects[jj][0])
self.assertAlmostEqual(obj[1], returned_objects[jj][1])
self.assertEqual(obj[2], returned_objects[jj][2])
def test_clear_objects(self):
"""Tests clear objects method of map"""
# set up objects to add to list (arbitrary values)
rng_list = [12.512, 44]
bearing_list = [-22, 81.5]
type_list = [ObjectType.BUOY, ObjectType.BOAT]
# add objects to object list 0.05s apart
start_time = dt.now()
for ii, (rng, bearing,
obj_type) in enumerate(zip(rng_list, bearing_list,
type_list)):
while (abs((dt.now() - start_time).total_seconds()) <
.05): # while less than 0.05s since last object
pass
obj = Object(bearing, rng, time_in_millis(), objectType=obj_type)
self.map.object_list.append(obj)
start_time = dt.now()
self.assertTrue(len(
self.map.object_list) == 2) # assert that length of list is two
while (abs(dt.now() - start_time).total_seconds() <
.05): # while less than 0.05s since last object
pass
self.map.clear_objects(
timeSinceLastSeen=75) # should only clear 2nd object
self.assertTrue(len(self.map.object_list) ==
1) # assert that length of list is only one
self.map.clear_objects(
timeSinceLastSeen=0) # should only clear all objects
self.assertTrue(len(
self.map.object_list) == 0) # assert that length of list is zero
def test_generate_obj_gate(self):
"""Tests generate obj gate method"""
# generate test object parameters
num_objects = 3
rng_list, bearing_list, obj_type_list = [5, 12, 40], [45, 0, -20], [
ObjectType.BOAT, ObjectType.NONE, ObjectType.BUOY
]
obj_list = [0] * num_objects
# initialize truthed gates
truth_rng_gates = [0] * num_objects
truth_bearing_gates = [0] * num_objects
truth_type_gates = [0] * num_objects
# create objects and set up truth gates
for ii in range(num_objects):
obj_list[ii] = Object(bearing_list[ii],
rng_list[ii],
time_in_millis(),
objectType=obj_type_list[ii])
obj_list[ii].kalman.covar = np.eye(4)
truth_rng_gates[ii] = (rng_list[ii] - 1, rng_list[ii] + 1)
truth_bearing_gates[ii] = (bearing_list[ii] - 1,
bearing_list[ii] + 1)
truth_type_gates[ii] = (ObjectType.NONE, obj_type_list[ii])
# compare truth gates to generated gates
truth_gates = [
*zip(truth_rng_gates, truth_bearing_gates, truth_type_gates)
]
for jj, obj in enumerate(obj_list):
gate = self.map._generate_obj_gate(obj)
self.assertEqual(truth_gates[jj], gate)
def test_return_objects(self):
"""Tests return objects method of map"""
# set up objects to add to map
timeRange = 5000 # time used to create rngRange
rngRange = (0, self.boat_speed * (timeRange / 1000)
) # range of ranges to return
bearingRange = (-30, 30) # range of bearings to return
type_list = [
ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY, ObjectType.BUOY,
ObjectType.BOAT, ObjectType.BUOY, ObjectType.NONE
] # object types
# set up objects
num_objects = 7
correct_object_list = [0] * num_objects # create empty object list
num_correct_objects = 0 # counter for correct objects
# loop over objects to create
for n in range(num_objects):
rng = (n * 6) + 3 # get range in range from 3 to 39
bearing = (n * 15) - 45 # get bearing in range from -45 to 45
# add object to correct object list if with rngRange and bearingRange
if (rngRange[0] <= rng <= rngRange[1]) and (
bearingRange[0] <= bearing <= bearingRange[1]):
correct_object_list[num_correct_objects] = [
rng, bearing, type_list[n]
]
num_correct_objects += 1
# add object to map
obj = Object(bearing,
rng,
time_in_millis(),
objectType=type_list[n])
self.map.object_list.append(obj)
# get list of objects meeting conditions
returned_objects = self.map.return_objects(bearingRange,
rngRange=rngRange)
# check that objects match
for jj, obj in enumerate(correct_object_list[0:num_correct_objects]):
self.assertAlmostEqual(obj[0], returned_objects[jj][0])
self.assertAlmostEqual(obj[1], returned_objects[jj][1])
self.assertEqual(obj[2], returned_objects[jj][2])
def test_smooth_frame(self):
"""Tests smooth frame method"""
# create objects to add to map
rng_list = [12.512, 44, 50]
bearing_list = [-22, 81.5, 2]
type_list = [ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY]
# loop through objects and add to map
for rng, bearing, obj_type in zip(rng_list, bearing_list, type_list):
obj = Object(bearing, rng, time_in_millis(), objectType=obj_type)
self.map.object_list.append(obj)
# create epoch frame
num_detects = 3
epoch_frame = [(12, -21, ObjectType.BUOY), (44, 80, ObjectType.BOAT),
(100, 100, ObjectType.BUOY)]
# create joint_pdaf return vals
truth_update_vals = [(12, -21), (44, 80), None]
truth_dets_used = [1, 1, 0]
with patch('src.tracking.map.joint_pdaf') as mock_pdaf, \
patch('src.tracking.map.Object.update') as mock_update, \
patch('src.tracking.map.Map._generate_obj_gate', return_value = 0), \
patch('src.tracking.map.Object.__init__', return_value = None) as mock_obj_init, \
patch('src.tracking.map.Map._return_full_objects', return_value = self.map.object_list), \
patch('src.tracking.map.time_in_millis', return_value = 1):
# set mock joint pdaf return value
mock_pdaf.return_value = truth_update_vals, truth_dets_used
# call smooth_frame
self.map.smooth_frame(epoch_frame, [0, 0])
# check that first two objects are correctly updated
for update_vals in filter(None, truth_update_vals):
mock_update.assert_any_call(*update_vals)
# check that new object is created for final detection
new_obj_idx = 2
self.assertEqual(
(epoch_frame[new_obj_idx][1], epoch_frame[new_obj_idx][0], 1),
mock_obj_init.call_args[0])
self.assertEqual({'objectType': epoch_frame[new_obj_idx][2]},
mock_obj_init.call_args[1])
def test_update_map(self, mock_predict):
"""Tests update map method"""
# add objects to list
num_objects = 2
rng_list = [12.512, 44]
bearing_list = [-22, 81.5]
type_list = [ObjectType.BUOY, ObjectType.BOAT]
for ii, (rng, bearing,
obj_type) in enumerate(zip(rng_list, bearing_list,
type_list)):
obj = Object(bearing, rng, time_in_millis(), objectType=obj_type)
self.map.object_list.append(obj)
# call update_map
self.map.update_map()
# check if predict was called for all objects in object_list
self.assertEqual(mock_predict.call_count, num_objects)
def smooth_frame(self, epoch_frame, frame_bounds):
"""
Updates map using observations from object list input
Inputs:
epoch_frame -- list containing rng, bearing, and object type for each detection in epoch
frame_bounds -- tuple of lists containing range and bearing bounds
Side Effects:
object_list -- Updates object list using data from frame (updates or creates new objects)
"""
# trim object list to only include tracks with frame bounds
trimmed_object_list = self._return_full_objects(bearingRange = frame_bounds[1], rngRange = frame_bounds[0])
if len(trimmed_object_list) != 0:
# generate gate list
gate_list = [self._generate_obj_gate(obj) for obj in trimmed_object_list]
# get update list from joint pdaf
update_list, detections_used = joint_pdaf(trimmed_object_list, gate_list, epoch_frame)
for obj, update in zip(trimmed_object_list, update_list):
# update objects
if update is not None:
mutex.acquire()
obj.update(update[0], update[1])
mutex.release()
else:
mutex.acquire()
obj.update(None, None)
mutex.release()
else:
detections_used = [0] * len(epoch_frame)
# use all detections NOT used to update objects to create new objects
for ii, det in enumerate(epoch_frame):
if detections_used[ii] == 0:
new_obj = Object(det[1], det[0], time_in_millis(), objectType = det[2]) # create object using detection
mutex.acquire()
self.object_list.append(new_obj) # add to object_list
mutex.release()
def setUp(self):
"""Sets up the objects needed for testing"""
self.object = Object(0, 0) # empty object
class ObjectTests(unittest.TestCase):
"""Tests the methods in Object"""
def setUp(self):
"""Sets up the objects needed for testing"""
self.object = Object(0, 0) # empty object
def test_set_object_state(self):
"""Tests set object state method of Object"""
# set kalman state to arbitrary choice of numbers
rb = [2, 45]
rb_prime = [5, 5]
self.object.kalman.state = np.array(rb + rb_prime)
# call set object state method
self.object._set_object_state()
# ensure that object state is set correctly
rng, bearing = rb
rngRate, bearingRate = rb_prime
self.assertAlmostEqual(self.object.rng, rng)
self.assertAlmostEqual(self.object.bearing, bearing)
self.assertAlmostEqual(self.object.rngRate, rngRate)
self.assertAlmostEqual(self.object.bearingRate, bearingRate)
@patch('src.tracking.object.time_in_millis')
def test_find_object_rngRate(self, mock_time_in_millis):
"""Tests find object rngRate method of Object"""
# set rng, time_in_millis, and lastSeen to arbitrary values
rng = 10.
lastSeen = 0
millis_time = 1000
self.object.rng = rng
self.object.lastSeen = lastSeen
mock_time_in_millis.return_value = millis_time
# call find object rngRate method
self.object._find_object_rngRate()
# ensure that return value is correct
rngRate = 1000. * (rng - 0) / (millis_time - lastSeen)
self.assertAlmostEqual(rngRate, self.object.rngRate)
@patch('src.tracking.object.time_in_millis')
def test_find_object_bearringRate(self, mock_time_in_millis):
"""Tests find object bearingRate method of Object"""
# set bearing, time_in_millis, and lastSeen to arbitrary values
bearing = 5.
lastSeen = 0
millis_time = 1000
self.object.bearing = bearing
self.object.lastSeen = lastSeen
mock_time_in_millis.return_value = millis_time
# call find object rngRate method
self.object._find_object_bearingRate()
# ensure that return value is correct
bearingRate = 1000. * (bearing - 0) / (millis_time - lastSeen)
self.assertAlmostEqual(bearingRate, self.object.bearingRate)
@patch('src.tracking.object.time_in_millis')
@patch('src.tracking.object.Object._find_object_bearingRate')
@patch('src.tracking.object.Object._find_object_rngRate')
@patch('src.tracking.object.Object._set_object_state')
@patch('src.tracking.object.KalmanFilter.update')
def test_update(self, mock_kalman_update, mock_set_obj_state, mock_find_rngRate,
mock_find_bearingRate, mock_time_in_millis):
"""Tests update method of Object"""
# set mocks and states to arbitrary values for testing
rng, bearing = 2, 3
rngRate, bearingRate = 0, 0
self.object.rngRate, self.object.bearingRate = rngRate, bearingRate
kalman_state = np.array([rng, bearing, 0, 0])
self.object.kalman.state = kalman_state # used by _set_object_state
time_in_millis_val = 5
mock_time_in_millis.return_value = time_in_millis_val
# call update
self.object.update(rng, bearing)
# hist score (with one detection in hist)
# max_val, min_val = 1.05, 0.95
# scale_fac = (max_val - min_val) / self.object.histLength
# hist_score = max_val - ((1+4.5)*scale_fac)
# ensure proper behavior
mock_kalman_update.assert_called_with([rng, bearing], [rngRate, bearingRate])
mock_set_obj_state.assert_called_once_with()
mock_find_rngRate.assert_called_once_with()
mock_find_bearingRate.assert_called_once_with()
self.assertEqual(time_in_millis_val, self.object.lastSeen)
# check update history behavior
truth_history = [None] * self.object.histLength
truth_history[0] = 1
self.assertEqual(truth_history, self.object.updateHist)
# repeat test with non-None rngRate and bearingRate
# reset all mocks
mock_kalman_update.reset_mock()
mock_set_obj_state.reset_mock()
mock_find_rngRate.reset_mock()
mock_find_bearingRate.reset_mock()
mock_time_in_millis.reset_mock()
# set mocks and states to arbitrary values for testing
rng, bearing = 2, 3
rngRate, bearingRate = 5, 90
kalman_state = np.array([rng, bearing, rngRate, bearingRate])
self.object.kalman.state = kalman_state # used by _set_object_state
time_in_millis_val = 5
mock_time_in_millis.return_value = time_in_millis_val
# call update
self.object.update(rng, bearing, rngRate, bearingRate)
# ensure proper behavior
mock_kalman_update.assert_called_with([rng, bearing], [rngRate, bearingRate])
mock_set_obj_state.assert_called_once_with()
mock_find_rngRate.assert_called_once_with()
mock_find_bearingRate.assert_called_once_with()
self.assertEqual(time_in_millis_val, self.object.lastSeen)
# check update history behavior
truth_history[1] = 1
self.assertEqual(truth_history, self.object.updateHist)
# repeat test with None for rng and bearing (object not seen case)
# reset all mocks
mock_kalman_update.reset_mock()
mock_set_obj_state.reset_mock()
mock_find_rngRate.reset_mock()
mock_find_bearingRate.reset_mock()
mock_time_in_millis.reset_mock()
# call update
self.object.update(None, None)
# ensure proper behavior
mock_kalman_update.assert_not_called()
mock_set_obj_state.assert_not_called()
mock_find_rngRate.assert_not_called()
mock_find_bearingRate.assert_not_called()
# check update history behavior
truth_history[0:3] = [0, 1, 1]
self.assertEqual(truth_history, self.object.updateHist)
@patch('src.tracking.object.Object._set_object_state')
@patch('src.tracking.object.KalmanFilter.predict')
def test_predict(self, mock_predict, mock_set_obj_state):
"""Tests predict method of Object"""
# call predict
self.object.predict()
# ensure proper calls are made
mock_predict.assert_called_once_with()
mock_set_obj_state.assert_called_once_with()
def test_calc_hist_score(self):
"""Tests calc history score method of object"""
# set up hist vals and scores
update_hist_vals = [[1, None, None, None, None, None, None, None, None, None],
[0, 0, 0, 1, 1, 1, 0, 1, 0, 1],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]
hist_len = 10.
max_val = 1.
min_val = 0.
scale_fac = (max_val - min_val) / hist_len
# check for correct behavior
for hist_vals in update_hist_vals:
# calc hist score
num_nones = sum(el is None for el in hist_vals)
truth_hist_score = (sum(filter(None, hist_vals)) + (0.5 * num_nones)) * scale_fac
# set update hist
self.object.updateHist = hist_vals
# call calc hist score
self.object._calc_hist_score()
# check for correct behavior
self.assertAlmostEqual(truth_hist_score, self.object.histScore)
@patch('src.tracking.object.np.sum')
def test_calc_confidence(self, mock_sum):
"""Tests calc confidence method"""
# generate vals
hist_vals = [1., 0.9, 0., 0.75, 0]
sum_vals = [0., 20., 50., 100., 150.]
truth_confidence_vals = [1., 0.85, 0.25, 0.375, 0.]
# iterate through values
for hist_val, sum_val, truth_confidence in zip(hist_vals, sum_vals, truth_confidence_vals):
# set hist score
self.object.histScore = hist_val
# set mock val
mock_sum.return_value = sum_val
# call calc confidence
self.object._calc_confidence()
# check for correct behavior
self.assertAlmostEqual(truth_confidence, self.object.confidence)