def _sync_time_steps(self):
# if self._use_truth:
# # current cells, previous cells
# cost_matrix = many_to_many_dists(self._curr_cells, self._prev_cells)
#
# # current cells, previous cells
# assignments = _hungarian(cost_matrix)
#
# # if cells are indistinguishable, create "shadow" cells as temporary replacements
# if self._prev_cells.shape[0] > assignments.shape[0]:
# self._create_shadow_cells(assignments)
# else:
# self._prev_cells = self._prev_cells[assignments[:, 1]]
# self._particles_all = self._particles_all[assignments[:, 1]]
# else:
# new_prev_cells = np.zeros(shape=self._curr_cells.shape, dtype="float32")
# new_prev_cells[:self._prev_cells.shape[0]] = self._prev_cells
# self._prev_cells = new_prev_cells
# # previous cells, current cells
# cost_matrix = many_to_many_dists(self._prev_cells, self._curr_cells)
#
# # previous cells, current cells
# assignments = _hungarian(cost_matrix)
# print(self._prev_cells.shape, self._curr_cells.shape)
# print(assignments)
# num_assigned = assignments.shape[0]
# if cells are indistinguishable, create "shadow" cells as temporary replacements
if self._curr_cells.shape[0] < self._prev_cells.shape[0]:
cost_matrix = many_to_many_dists(self._curr_cells, self._prev_cells)
assignments = _hungarian(cost_matrix)
self._create_shadow_cells(assignments)
else:
# previous cells, current cells
cost_matrix = many_to_many_dists(self._prev_cells, self._curr_cells)
# previous cells, current cells
assignments = _hungarian(cost_matrix)
# print(self._prev_cells.shape, self._curr_cells.shape)
# print(assignments)
num_assigned = assignments.shape[0]
if self._curr_cells.shape[0] > self._prev_cells.shape[0]:
mask = np.zeros(shape=(self._curr_cells.shape[0],), dtype="bool")
mask[assignments[:, 1]] = True
# print(self._curr_cells[~mask])
# print(self._prev_cells.shape, self._curr_cells.shape)
new_curr_cells = np.zeros(shape=self._prev_cells.shape, dtype="float32")
new_curr_cells[:num_assigned] = self._curr_cells[assignments[:, 1]]
# new_curr_cells[num_assigned:] = self._curr_cells[~mask]
self._curr_cells = new_curr_cells
# print(self._curr_cells)
else:
self._curr_cells = self._curr_cells[assignments[:, 1]]
def test_hungarian():
matrices = [
# Square
([[400, 150, 400],
[400, 450, 600],
[300, 225, 300]],
850 # expected cost
),
# Rectangular variant
([[400, 150, 400, 1],
[400, 450, 600, 2],
[300, 225, 300, 3]],
452 # expected cost
),
# Square
([[10, 10, 8],
[9, 8, 1],
[9, 7, 4]],
18
),
# Rectangular variant
([[10, 10, 8, 11],
[9, 8, 1, 1],
[9, 7, 4, 10]],
15
),
# n == 2, m == 0 matrix
([[], []],
0
),
]
for cost_matrix, expected_total in matrices:
cost_matrix = np.array(cost_matrix)
indexes = _hungarian(cost_matrix)
total_cost = 0
for r, c in indexes:
x = cost_matrix[r, c]
total_cost += x
assert expected_total == total_cost
indexes = _hungarian(cost_matrix.T)
total_cost = 0
for c, r in indexes:
x = cost_matrix[r, c]
total_cost += x
assert expected_total == total_cost
def test_hungarian():
from sklearn.utils.linear_assignment_ import _hungarian
matrices = [
# Square
(
[[400, 150, 400], [400, 450, 600], [300, 225, 300]],
850 # expected cost
),
# Rectangular variant
(
[[400, 150, 400, 1], [400, 450, 600, 2], [300, 225, 300, 3]],
452 # expected cost
),
# Square
([[10, 10, 8], [9, 8, 1], [9, 7, 4]], 18),
# Rectangular variant
([[10, 10, 8, 11], [9, 8, 1, 1], [9, 7, 4, 10]], 15),
# n == 2, m == 0 matrix
([[], []], 0),
]
for cost_matrix, expected_total in matrices:
cost_matrix = np.array(cost_matrix)
indexes = _hungarian(cost_matrix)
total_cost = 0
for r, c in indexes:
x = cost_matrix[r, c]
total_cost += x
assert expected_total == total_cost
indexes = _hungarian(cost_matrix.T)
total_cost = 0
for c, r in indexes:
x = cost_matrix[r, c]
total_cost += x
assert expected_total == total_cost
def _sync_time_steps(self):
# current cells, previous cells
cost_matrix = many_to_many_dists(self._curr_cells, self._prev_cells)
# current cells, previous cells
assignments = _hungarian(cost_matrix)
# if cells are indistinguishable
if self._prev_cells.shape[0] > assignments.shape[0]:
self._create_shadow_cells(assignments)
else:
self._prev_cells = self._prev_cells[assignments[:, 1]]
self._particles_all = self._particles_all[assignments[:, 1]]
def hungarianassociation(loss, threshold):
print loss
# SKLEARN association method
res = _hungarian(loss)
del_index = []
for ii in range(len(res)):
y, x = res[ii]
"""
if(loss[y, x] > threshold):
del_index.append(ii)
"""
new_res = np.delete(res, del_index, 0)
return new_res
def hungarianassociation(loss, distance, threshold):
loss = postproc(loss, threshold)
debug_flag = 0
if debug_flag: print loss
# SKLEARN association method
res = _hungarian(loss)
del_index = []
for ii in range(len(res)):
y, x = res[ii]
if(loss [y, x] > threshold):
del_index.append(ii)
new_res = np.delete(res, del_index, 0)
#print new_res
return new_res
def hungarianassociation(loss, distance, threshold):
loss = postproc(loss, threshold)
debug_flag = 0
if debug_flag: print loss
# SKLEARN association method
res = _hungarian(loss)
del_index = []
for ii in range(len(res)):
y, x = res[ii]
if (loss[y, x] > threshold):
del_index.append(ii)
new_res = np.delete(res, del_index, 0)
#print new_res
return new_res
def updateOutput(self, dictionary, scores, ys):
qs = dictionary
ss = dictionary
original_dimensionality = qs.shape[0]
elements_prediction = qs.shape[1]
elements_gt = 0
ys_size = tf.size(ys)
if ys_size is not 0:
elements_gt = ys.shape[1]
zero = tf.zeros([1])
one = tf.ones([1])
M_size = elements_gt
M_tensor = tf.zeros([M_size, M_size])
M = M_tensor.eval()
ys = ys.eval()
qs = dictionary.eval()
ss = scores.eval()
for i in range(0, min(elements_prediction, elements_gt)):
for j in range(0, elements_gt):
M[i, j] = -self.IoU(qs[:, i], ys[:, j])
temp = self.lambda_value * (
tf.nn.sigmoid_cross_entropy_with_logits(
logits=tf.convert_to_tensor(ss[i, :]), labels=one))
M[i, j] = M[i, j] + temp.eval()
if elements_gt > 0:
self.assignments = _hungarian(M)
output = 0
# Save May Perform #
for row, column in self.assignments:
output = output + M[row, column]
for i in range(elements_gt + 1, elements_prediction):
temp = self.lambda_value * tf.nn.sigmoid_cross_entropy_with_logits(
logits=tf.convert_to_tensor(ss[i, :]), labels=zero)
output = output + temp.eval()
return output
if ifn == 0:
dets_prev = dets
else:
# determine initial location and heading from dets and dets_prev
if dets.size > 0:
# create cost array from detections
cost = []
for a in dets:
dist = [np.linalg.norm(a - b) for b in dets_prev]
cost.append(dist)
# associate detections
result = _hungarian(np.array(cost))
for ii in range(len(result)):
cidx, pidx = result[ii]
robot_pos = dets[cidx][::-1] # expects (x,y) instead of (y,x)
robot_pos_prev = dets_prev[pidx][::-1]
#print('Position:', robot_pos, 'Position_prev:', robot_pos_prev)
heading = angle_between(robot_pos_prev, robot_pos)
speed = np.linalg.norm(robot_pos - robot_pos_prev)
particles = particles_list[pidx]
weights = weights_list[pidx]
# distance from robot to each landmark
zs = (norm(landmarks - robot_pos, axis=1) +
(randn(NL) * sensor_std_err))
