def matching(L1, L2):
sim_matrix = np.zeros((len(L1), len(L2)))
for i, w1 in enumerate(L1):
for j, w2 in enumerate(L2):
sim_matrix[i][j] = viwordnet.path(w1, w2)
row_ind, col_ind = lsa(-sim_matrix)
return sim_matrix[row_ind, col_ind].tolist(), row_ind.tolist(), col_ind.tolist()
def get_results(filename):
print(filename)
_dir, _, _file = filename.rpartition('/')
_image, _, _ext = _file.rpartition('.')
_annots, _, _im_ext = _image.rpartition('.')
files.append(_image)
# print _image
# grab the train and test annotations
trains = np.load(os.path.join(args.train_dir, _annots + _ + _ext)).item()
tests = np.load(filename)
#print filename, maps_dir+_image, args.train_dir+_annots+_+_ext
# data structure for saving the IoU values
IoU_vals = np.zeros((len(tests), len(trains.keys())))
# save the train anots
train_polys = []
for i in trains.keys():
# print(trains[i]['vertices'])
train_polys.append(Polygon(trains[i]['vertices']))
pass
s = STRtree(train_polys)
# save the test annots
test_polys = []
for i in range(len(tests)):
poly = tests[i]
poly = poly.tolist()
# poly.append(poly[0])
test_poly = Polygon(poly)
if not test_poly.is_valid:
continue
try:
results = s.query(test_poly)
for j in range(len(results)):
_id = train_polys.index(results[j])
_intersection = train_polys[_id].intersection(test_poly).area
_union = train_polys[_id].union(test_poly).area
IoU_vals[i, _id] = _intersection / _union
test_polys.append(test_poly)
except Exception:
continue
# do the linear sum assignment
_row, _col = lsa(1 - IoU_vals)
assignment_matrix = IoU_vals[_row, _col]
# compute the numbers
TP = (assignment_matrix >= IoU_thresh).sum()
FP = (assignment_matrix < IoU_thresh).sum()
FN = len(trains.keys()) - TP
return [TP, FP, FN]
def match_atom(x, D):
'''Find least cost assignment from x to an atom of D.'''
cost = np.zeros(D.shape[1])
assignment = dict()
for ii in trange(D.shape[1], leave=False):
C = cdist(x[:, None], D[:, ii][:, None])
rows, cols = lsa(C)
assignment[ii] = cols
cost[ii] = C[rows, cols].sum()
jj = np.argmin(cost)
return (jj, assignment[jj])
def obj1(c0, x, lam, norm):
'''Find the cost between current xhat and x using lsa.
Notes
-----
This bakes the cost of linear sum assignment right into the
objectve function and adds a regularizing l1 term to encourage
the solution to be sparse.
'''
xhat = make_xhat(c0, norm)
C = cdist(xhat[:, None], x[:, None])
row, col = lsa(C)
return C[row, col].sum() + lam * np.linalg.norm(c0, ord=1)
def acc(targets, predictions, k=None):
assert len(targets) == len(predictions)
# targets = torch.tensor(targets)
# predictions = torch.tensor(predictions)
n = len(targets)
if k is None:
k = targets.max() + 1
cost = torch.zeros(k, k)
for i in range(n):
cost[targets[i].item(), predictions[i].item()] -= 1
stuff = -cost[lsa(cost)]
total = stuff.sum().item() / n
for i in range(k):
stuff[i] /= -cost[i].sum()
return total, stuff
def accuracy_score(y_act, y_pred):
Cluster_Matrix = np.zeros((max(y_act) + 1, max(y_pred) + 1))
print "Cluster_Matrix shape:"
for i in range(len(y_act)):
Cluster_Matrix[y_act[i], y_pred[i]] += 1.0
Inv_Cluster_Matrix = -1 * Cluster_Matrix
row, col = lsa(Inv_Cluster_Matrix)
accu = np.sum(Cluster_Matrix[row, col]) / float(
np.sum(np.sum(Cluster_Matrix)))
for i, j in itertools.izip(row, col):
print(i, j,
np.sum(Cluster_Matrix[i, :]) / np.sum(np.sum(Cluster_Matrix)),
Cluster_Matrix[i, j] / float(np.sum(Cluster_Matrix[:, j])))
print(Cluster_Matrix.astype(int))
print(row)
print(col)
return accu
def get_assignment_matrix(distance_matrix):
"""
Get hungarian assignment using scipy's linear_sum_assignment
Parameters:
----------
distance_matrix: `numpy.ndarray`
N x M array with distances of N trackers and M detections
Returns:
--------
rows: `numpy.ndarray`
sorted list of indices of trackers (starting with 0,1,2...)
cols: `numpy.ndarray`
array of indices of respective assigned boxes to the trackers
"""
rows, cols = lsa(distance_matrix)
return rows, cols
def maxcost(u, v):
r"""
Solves the maximum bipartite matching problem for the bipartite graph with
vertex sets :math:`A=\{a_1,\dots,a_k\}` and :math:`B=\{b_1,\dots,b_k\}`
such that the edge :math:`(a_i, b_j)` has weight :math:`|u_i-v_j|`, with
:math:`u,v` distribution vectors in the unit :math:`k`-cube. Tells us the
maximum sum of squared differences over all permutations of coordinates in
:math:`u` and :math:`v`.
:param list u: Vector.
:param list v: Vector.
:return: Maximum sum of squared differences.
:rtype: float
"""
# Create a cost matrix for the linear sum assignment method.
C = np.array([[(u[i] - v[j])**2 for j in range(len(v))]
for i in range(len(u))])
return np.sqrt(C[lsa(C, maximize=False)].sum())
def agree(dict1, dict2):
"""
:param dict1: is a dict containing a set of lists for every key
:param dict2: is a dict containing a set of lists for every key
:return: an agreement score as to how both sets are similar to each other
"""
M = np.empty((len(dict1), len(dict2)))
M[:] = np.nan
for k_i, v_i in dict1.items():
for k_j, v_j in dict2.items():
M[k_i, k_j] = jaccardSim_avg(v_i, v_j)
# M is a 'profit' matrix. It has to be inverted to a 'cost' matrix to find the min
M_invert = 1 - M
# Extract the index of the minimum values (hence max value) using the Hungarian Method
rowIndex, colIndex = lsa(M_invert)
jSim_max = M[rowIndex, colIndex]
return sum(jSim_max) / len(dict1)
def matchBbox(bbox0, bbox1):
# Hungarian matching of the bipartite graph between two sets of bounding boxes
# weight (cost) is the distance between bounding boxes
# if #row > #col, it will return #row of indices, and vice versa
cost_matrix = np.zeros((len(bbox0), len(bbox1)))
for row, bbox_gt_ in enumerate(bbox0):
center_x_gt = bbox_gt_[0] + bbox_gt_[2] / 2
center_y_gt = bbox_gt_[1] + bbox_gt_[3] / 2
for col, bbox_eff_ in enumerate(bbox1):
center_x_eff = bbox_eff_[0] + bbox_eff_[2] / 2
center_y_eff = bbox_eff_[1] + bbox_eff_[3] / 2
cost_matrix[row, col] = ((center_x_gt - center_x_eff)**2 +
(center_y_gt - center_y_eff)**2)**0.5
row_ind, col_ind = lsa(cost_matrix)
return row_ind, col_ind
def fit(self, X, Y):
"""
Fits transformation matrices (Wx, Wy) and, if
center_columns == True, bias terms (mx_, my_).
Parameters
----------
X : ndarray
(num_samples x num_neurons) matrix of activations.
Y : ndarray
(num_samples x num_neurons) matrix of activations.
"""
X, Y = check_equal_shapes(X, Y, nd=2, zero_pad=self.zero_pad)
if self.center_columns:
self.mx_ = mx = np.mean(X, axis=0)
self.my_ = my = np.mean(Y, axis=0)
X = X - mx[None, :]
Y = Y - mx[None, :]
self.Px_, self.Py_ = lsa(X.T @ Y, maximize=True)
return self
def correlate_clusters(c1, c2, and_mapping=False, mean_only=True):
from scipy.optimize import linear_sum_assignment as lsa
K = c1.shape[0]
#get the correlation between all the clusters
#in a matrix
corrs = np.zeros([K, K])
weights = cosine_latitude_weights(c1[0])
for i, j in itertools.product(np.arange(K), repeat=2):
corrs[i, j] = ipearsonr(c1[i], c2[j], weights=weights).data
order = lsa(-corrs)
mapping = [(a, b) for a, b in zip(order[0], order[1])]
pattern_corrs = np.array([corrs[map_] for map_ in mapping])
mean_corrs = pattern_corrs.mean()
if mean_only:
corrs = mean_corrs
else:
corrs = (mean_corrs, pattern_corrs)
if and_mapping == True:
return corrs, mapping
else:
return corrs
def linear_assignment(row_list, col_list, costMatrix):
"""
Compute the linear assignment between two label sets.
Parameters:
- - - - -
row_list : list of label values in non-moving brain
col_list : list of label values in moving brain
costMatrix : matrix of costs between labels in each brain
"""
# Compute linear assignment
ar, ac = lsa(costMatrix)
rows = row_list[ar]
cols = col_list[ac]
non_mapped = set(col_list).difference(set(cols))
# Remap assignment indices to true label values
remapped = dict(zip(rows, cols))
unmapped = list(non_mapped)
return remapped, unmapped
global_IoU_vals.append(IoU_vals) trains_all.append(train_polys) test_all.append(test_polys) #break print "done computing the IoUs" #''' for IoU_thresh in np.arange(0, 1+step, step): print IoU_thresh stats = [] for i in range(len(global_IoU_vals)): IoU_vals = np.copy(global_IoU_vals[i]) _id = IoU_vals < IoU_thresh IoU_vals[_id] = 0 _row, _col = lsa(1-IoU_vals) TP = (IoU_vals[_row, _col] > 0).sum() FP = (IoU_vals[_row, _col] == 0).sum() # can't have true negative FN = len(trains_all[i]) - TP stats.append([TP, FP, FN]) global_stats.append(stats) stats = np.asarray(stats) avg_TP = float(stats[:,0].sum()) / float(stats.shape[0]) avg_FP = float(stats[:,1].sum()) / float(stats.shape[0]) avg_FN = float(stats[:,2].sum()) / float(stats.shape[0])
def ordinator1d(prior,
k,
forward,
inverse,
chunksize=10,
pdf=None,
pdf_metric=None,
sparse_metric=None,
disp=False):
'''Find permutation that maximizes sparsity of 1d signal.
Parameters
----------
prior : array_like
Prior signal estimate to base ordering.
k : int
Desired sparsity level.
forward : callable
Sparsifying transform.
inverse : callable
Inverse sparsifying transform.
chunksize : int, optional
Chunk size for parallel processing pool.
pdf : callable, optional
Function that estimates pixel intensity distribution.
pdf_metric : callable, optional
Function that returns the distance between pdfs.
sparse_metric : callable, optional
Metric to use to measure sparsity. Uses l1 norm by default.
disp : bool, optional
Whether or not to display coefficient plots at the end.
Returns
-------
array_like
Reordering indices.
Raises
------
ValueError
If disp=True and forward function is not provided.
Notes
-----
pdf_method=None uses histogram. pdf_metric=None uses l2 norm. If
disp=True then forward transform function must be provided.
Otherwise, forward is not required, only inverse.
pdf_method should assume the signal will be bounded between
(-1, 1). We do this by always normalizing a signal before
computing pdf or comparing.
'''
# # Make sure we have the forward transform if we want to display
# if disp and forward is None:
# raise ValueError(
# 'Must provide forward transform for display!')
# Make sure we have a sparsity metric
if sparse_metric is None:
sparse_metric = lambda x0: np.linalg.norm(x0, ord=1)
# Make sure we do in fact have a 1d signal
if prior.ndim > 1:
logging.warning('Prior is not 1d! Flattening!')
prior = prior.flatten()
N = prior.size
# Go ahead and normalize the signal so we don't have to keep
# track of the limits of the pdfs we want to compare, always
# between (-1, 1).
prior /= np.max(np.abs(prior)) + np.finfo('float').eps
# Default to histogram
if pdf is None:
pdf_object = pdf_default(prior)
pdf = pdf_object.pdf
pdf_ref = pdf_object.pdf_ref
else:
# Get reference pdf
pdf_ref = pdf(prior)
# Default to l2 metric
if pdf_metric is None:
pdf_metric = pdf_metric_default
# Let's try to do things in parallel -- more than twice as fast!
search_fun_partial = partial(search_fun,
N=N,
k=k,
inverse=inverse,
pdf_ref=pdf_ref,
pdf_metric=pdf_metric,
pdf=pdf)
t0 = time() # start the timer
with Pool() as pool:
res = list(
tqdm(pool.imap(search_fun_partial, combinations(range(N), k),
chunksize),
total=comb(N, k, exact=True),
leave=False))
res = np.array(res)
# Choose the winner
winner_idx = np.where(res[:, -1] == res[:, -1].min())[0]
potentials = []
for idx0 in winner_idx:
potentials.append(res[idx0, :])
print('potential:', potentials[-1][0])
print('Found %d out of %d (%%%g) potentials in %d seconds!' %
(len(potentials), res.shape[0], len(potentials) / res.shape[0] * 100,
time() - t0))
# Now solve the assignment problem, we only need one of the
# potentials, so look at all of them and choose the one that is
# most sparse
if disp:
import matplotlib.pyplot as plt
winner_sparse = np.inf
cur_win = None
for potential in potentials:
c = np.zeros(N)
idx_proposed = potential[0]
c[idx_proposed] = potential[1]
xhat = inverse(c)
xhat /= np.max(np.abs(xhat)) + np.finfo('float').eps
C = cdist(xhat[:, None], prior[:, None])
_rows, cols = lsa(C)
cur_sparse = sparse_metric(forward(prior[cols]))
if cur_sparse < winner_sparse:
winner_sparse = cur_sparse
print('New win: %g' % cur_sparse)
cur_win = cols
if disp:
tcoeffs = np.abs(forward(prior[cols]))
# tcoeffs /= np.max(tcoeffs)
plt.plot(-np.sort(-tcoeffs), '--', label='xpi')
# Show reference coefficients
if disp:
# plt.plot(-np.sort(-np.abs(forward(xhat))), label='xhat')
tcoeffs = np.abs(forward(np.sort(prior)))
# tcoeffs /= np.max(tcoeffs)
plt.plot(-np.sort(-tcoeffs), ':', label='sort(x)')
plt.legend()
plt.title('Sorted, Normalized Transform Coefficients')
plt.show()
return cur_win
def hung_dedup(df, return_dups=False, dups=False):
if not isinstance(df, pd.DataFrame):
df = pd.read_csv(prediction_file)
if dups:
just_dups = df
else:
a = df[df.columns[0]]
b = df[df.columns[1]]
g = df[a.isin(a[a.duplicated(keep=False)])]
h = df[b.isin(b[b.duplicated(keep=False)])]
just_dups = pd.concat([g, h], ignore_index=True).drop_duplicates(
subset=[df.columns[0], df.columns[1]])
just_dups = just_dups.loc[:,
~just_dups.columns.str.contains('^Unnamed')]
index1, index2 = list(set(just_dups.iloc[:, 0])), list(
set(just_dups.iloc[:, 1]))
col1, col2, col3 = list(just_dups.iloc[:, 0]), list(
just_dups.iloc[:, 1]), list(just_dups.iloc[:, 2])
k = len(index1)
ind_ind1 = dict(zip(index1, list(range(k))))
j = k + len(index2)
ind_ind2 = dict(zip(index2, list(range(k, j))))
inv_ind1 = {v: k for k, v in ind_ind1.items()}
inv_ind2 = {v: k for k, v in ind_ind2.items()}
row, col, data = [], [], []
row = [ind_ind1[x] for x in col1]
col = [ind_ind2[x] for x in col2]
data = list(-1 * np.array(just_dups.iloc[:, 2]))
A = csc_matrix((data, (row, col)), shape=(j, j))
jl = list(range(j))
n, f = csgraph.connected_components(A, directed=False, connection='weak')
l = {
k: set(map(lambda x: x[1], v))
for k, v in itertools.groupby(sorted(zip(f, jl)), key=lambda x: x[0])
}
pairs = list(zip(row, col))
get_prob = dict(zip(pairs, data))
num_connected_sets = len(l)
y1ind, y2ind = set(range(k)), set(range(k, j))
pairs = set(pairs)
year1, year2, prob = [], [], []
for i in range(num_connected_sets):
s = l[i]
y1 = list(s & y1ind)
y2 = list(s & y2ind)
mat = np.zeros((len(y1), len(y2)))
q = itertools.product(y1, y2)
for a in q:
if a in pairs:
one, two = a
mat[y1.index(one), y2.index(two)] = get_prob[a]
r, c = lsa(mat)
for j in range(len(c)):
a = (y1[r[j]], y2[c[j]])
if a in pairs:
year1 += [inv_ind1[y1[r[j]]]]
year2 += [inv_ind2[y2[c[j]]]]
prob += [get_prob[a]]
prob = list(-1 * np.array(prob))
final = pd.DataFrame(
list(zip(year1, year2, prob)),
columns=[just_dups.columns[0], just_dups.columns[1], 'link_prob'])
if return_dups:
return final, just_dups
else:
return final
def get_action(self, agent_list, target_list):
agent_pos_list = np.array([agent.pos for agent in agent_list])
target_pos_list = np.array([target.pos for target in target_list])
target_time_list = np.array([target.time for target in target_list])
if len(target_pos_list) == 0:
return [0] * len(agent_pos_list)
agent2target_list = []
for i, agent_pos in enumerate(agent_pos_list):
agent2target_list.append([])
for target_pos, target_time in zip(target_pos_list,
target_time_list):
distVtime = np.sum(np.abs(target_pos - agent_pos))
agent2target_list[i].append(distVtime)
target_for_agent = []
cost = np.array(agent2target_list)
if len(target_list) >= 2:
_, target_for_agent = lsa(cost)
elif 0 < len(target_list) < len(agent_pos_list):
cost = np.hstack((cost, 1000 * np.ones((len(agent_pos_list), 1))))
_, target_for_agent = lsa(cost)
else:
target_for_agent = [-1] * len(agent_pos_list)
action_list = []
for agent_idx, target_idx in enumerate(target_for_agent):
if (target_idx == -1 or target_idx > len(target_list) - 1
or np.sum(
np.abs(target_pos_list[target_idx] -
agent_pos_list[agent_idx])) >
target_time_list[target_idx]):
action_list.append(0)
else:
x, y = target_pos_list[target_idx] - agent_pos_list[agent_idx]
if x == 0 and y == 0:
action_list.append(0)
elif x == 0 or y == 0:
if not x == 0:
if x > 0:
action_list.append(1)
else:
action_list.append(2)
elif not y == 0:
if y > 0:
action_list.append(4)
else:
action_list.append(3)
else: # both are non zero
choice = np.random.randint(0, 2)
if choice == 0:
if x > 0:
action_list.append(1)
else:
action_list.append(2)
else:
if y > 0:
action_list.append(4)
else:
action_list.append(3)
return action_list
def relaxed_ordinator(x,
lam,
k,
unsparsify,
norm=False,
warm=False,
transform_shape=None,
disp=False):
'''Find ordering pi that makes x[pi] sparse.
Parameters
----------
x : array_like
Signal to find ordering of.
lam : float
Lagrangian weight on l1 term of objective function.
k : int
Expected sparsity level (number of nonzero coefficients) of
ordererd signal, x[pi].
unsparsify : callable
Function that computes inverse sparsifying transform.
norm : bool, optional
Normalize xhat at each step (probably don't do this.)
warm : bool
Whether to look for warm start file and save intermedate
results.
transform_shape : int
Shape of transform coefficients (if different than x.shape).
None will use x.shape.
disp : bool
Display progress messages.
Returns
-------
pi : array_like
Flattened ordering array (like is returned by numpy.argsort).
Notes
-----
`size_transform` will be x.size - 1 for finite differences
transform.
'''
# If size of coefficients is different than x.shape, make note
if transform_shape is None:
transform_shape = x.shape
# Check to see if we can warm start
c0 = load_intermediate()
if not warm or c0 is None:
c0 = np.ones(transform_shape).flatten()
else:
print('WARM START')
pobj = partial(obj,
x=x,
lam=lam,
unsparsify=unsparsify,
norm=norm,
transform_shape=transform_shape)
res = minimize(pobj,
c0,
callback=lambda x: save_intermediate(x, pobj(x), disp))
# print(res)
# Go ahead and hard threshold here
c_est = res['x']
c_est[np.abs(c_est) < np.sort(np.abs(c_est))[-k]] = 0
# plt.plot(res['x'])
# plt.plot(c_true)
# plt.show()
# Do the assignment and try to recover x
xhat = make_xhat(c_est.reshape(transform_shape), unsparsify, norm)
C = cdist(xhat.flatten()[:, None], x.flatten()[:, None])
_row, pi = lsa(C)
return pi
def hungarian(A):
B = A.T
rows, cols = lsa(B)
return cols, sum(B[rows, cols])
def error(self, variables):
########## Expectation ##########
pose = variables.at(self.keys()[0])
location = np.append(pose.translation(), self.z) # append z
orientation = np.array([0, 0, pose.so2().theta()])
if self._first_time:
# Store the initially guessed pose when computing error the first time
self._init_tform = Transform.from_conventional(
location, orientation)
self._init_orientation = orientation
if self.static:
# Static mode
if self._first_time:
# First time extracting expected land boundaries
fbumper_location = get_fbumper_location(
location, orientation, self.px)
self.in_junction, self.into_junction, self.me_format_expected_markings = self.expected_lane_extractor.extract(
fbumper_location, orientation)
expected_coeffs_list = [expected.get_c0c1_list()
for expected in self.me_format_expected_markings]
expected_type_list = [expected.type
for expected in self.me_format_expected_markings]
# The snapshot is stored in their normal forms; i.e. a, b, c, and alpha describing the lines
self._init_normal_forms = [compute_normal_form_line_coeffs(self.px, c[0], c[1])
for c in expected_coeffs_list]
# Snapshot of lane boundary types
self._init_types = expected_type_list
self._first_time = False
else:
# Not first time, use snapshot of lane boundaries extracted the first time to compute error
# Pose difference is wrt local frame
pose_diff = self._get_pose_diff(location, orientation)
# Compute expected lane boundary coefficients using the snapshot
expected_coeffs_list = []
for normal_form in self._init_normal_forms:
c0c1 = self._compute_expected_c0c1(normal_form, pose_diff)
expected_coeffs_list.append(c0c1)
# Retrieve lane boundary types from snapshot
expected_type_list = self._init_types
else:
# Not static mode
# Extract ground truth from the Carla server
fbumper_location = get_fbumper_location(
location, orientation, self.px)
self.in_junction, self.into_junction, self.me_format_expected_markings = self.expected_lane_extractor.extract(
fbumper_location, orientation)
# List of expected markings' coefficients
expected_coeffs_list = [expected.get_c0c1_list()
for expected in self.me_format_expected_markings]
# List of expected markings' type
expected_type_list = [expected.type
for expected in self.me_format_expected_markings]
########## Measurement ##########
if self.left_marking:
measured_coeffs_left = np.asarray(
self.left_marking.get_c0c1_list()).reshape(2, -1)
measured_type_left = self.left_marking.type
if self.right_marking:
measured_coeffs_right = np.asarray(
self.right_marking.get_c0c1_list()).reshape(2, -1)
measured_type_right = self.right_marking.type
# Null hypothesis
# Use the measurement itself at every optimization iteration as the null hypothesis.
# This is, of course, just a trick.
# This means the error for null hypothesis is always zeros.
null_expected_c0c1_left = self.left_marking.get_c0c1_list()
null_expected_c0c1_right = self.right_marking.get_c0c1_list()
null_error = np.zeros((2, 1)) # same for both left and right
# Compute innovation matrix for the null hypo
null_noise_cov = self.noise_cov * self.null_std_scale**2
# Compute measurement likelihood weighted by null probability
null_weighted_meas_likelihood = self.prob_null * \
multivariate_normal.pdf(null_error.squeeze(), cov=null_noise_cov)
# In this implementation, scaling down error and jacobian is done to achieve
# the same effect of tuning the information matrix online.
# Here, however, scale down error for null hypo; i.e.
# null_error /= self.null_std_scale
# is not necessary, since its always zero.
# Zero error and jacobian effectively result in zero information matrix as well.
if self.ignore_junction and (self.in_junction or self.into_junction):
self._null_hypo_left = True
self._null_hypo_right = True
elif not expected_coeffs_list:
self._null_hypo_left = True
self._null_hypo_right = True
else:
# Data association
num_expected_markings = len(self.me_format_expected_markings)
asso_table = np.zeros((2, num_expected_markings+2))
# Left lane marking
errors_left = []
asso_probs = []
meas_likelihoods = []
for exp_coeffs, exp_type in zip(expected_coeffs_list, expected_type_list):
# Compute innovation matrix
expected_c0, expected_c1 = exp_coeffs
H = compute_H(self.px, expected_c0, expected_c1)
innov = H @ self.pose_uncert @ H.T + self.noise_cov
# Compute squared mahalanobis distance
error = np.asarray(exp_coeffs).reshape(
2, -1) - measured_coeffs_left
squared_mahala_dist = error.T @ np.linalg.inv(
innov) @ error
# Semantic likelihood
if self.semantic:
# Conditional probability on type
sem_likelihood = self._conditional_prob_type(
exp_type, measured_type_left)
else:
# Truning off semantic association is equivalent to always
# set semantic likelihood to 1.0
sem_likelihood = 1.0
# Gating (geometric and semantic)
# Reject both geometrically and semantically unlikely associations
# Note:
# Since location distribution across lanes is essentially multimodal,
# geometric gating often used when assuming location is unimodal is not
# very reasonable and will inevitablly reject possible associations.
# Here the geometric gate is set very large so we can preserve associations that
# are a bit far from the current mode (the only mode that exists in the optimization)
# but still possible when the multimodal nature is concerned.
# Or, we can simply give up geometric gating and use semantic gating only.
# The large geometric gate is an inelegant remedy after all.
# if squared_mahala_dist <= self.geo_gate and sem_likelihood > self.sem_gate:
if sem_likelihood > self.sem_gate:
errors_left.append(error)
# Measurement likelihood (based on noise cov)
meas_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=self.noise_cov)
# Geometric likelihood (based on innov)
geo_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=innov)
meas_likelihoods.append(meas_likelihood)
asso_prob = geo_likelihood * sem_likelihood
asso_probs.append(asso_prob)
else:
errors_left.append(None)
meas_likelihoods.append(0)
asso_probs.append(0)
asso_probs = np.asarray(asso_probs)
meas_likelihoods = np.asarray(meas_likelihoods)
# Compute weights based on total probability theorem
if asso_probs.sum():
weights_left = (1-self.prob_null) * \
(asso_probs/np.sum(asso_probs))
else:
weights_left = np.zeros(asso_probs.shape)
# Weight measurement likelihoods
weighted_meas_likelihood = weights_left*meas_likelihoods
# Add weight and weighted likelihood of null hypothesis
weights_left = np.insert(weights_left, 0, [self.prob_null, 0])
weighted_meas_likelihood = np.insert(
weighted_meas_likelihood, 0, [null_weighted_meas_likelihood, 0])
asso_table[0, :] = weighted_meas_likelihood
# Right marking
errors_right = []
asso_probs = []
meas_likelihoods = []
for exp_coeffs, exp_type in zip(expected_coeffs_list, expected_type_list):
# Compute innovation matrix
expected_c0, expected_c1 = exp_coeffs
H = compute_H(self.px, expected_c0, expected_c1)
innov = H @ self.pose_uncert @ H.T + self.noise_cov
# Compute squared mahalanobis distance
error = np.asarray(exp_coeffs).reshape(
2, -1) - measured_coeffs_right
squared_mahala_dist = error.T @ np.linalg.inv(
innov) @ error
# Semantic likelihood
if self.semantic:
# Conditional probability on type
sem_likelihood = self._conditional_prob_type(
exp_type, measured_type_right)
else:
# Truning off semantic association is equivalent to always
# set semantic likelihood to 1.0
sem_likelihood = 1.0
# Gating (geometric and semantic)
# Reject both geometrically and semantically unlikely associations
# Note:
# Since location distribution across lanes is essentially multimodal,
# geometric gating often used when assuming location is unimodal is not
# very reasonable and will inevitablly reject possible associations.
# Here the geometric gate is set very large so we can preserve associations that
# are a bit far from the current mode (the only mode that exists in the optimization)
# but still possible when the multimodal nature is concerned.
# Or, we can simply give up geometric gating and use semantic gating only.
# The large geometric gate is an inelegant remedy after all.
# if squared_mahala_dist <= self.geo_gate and sem_likelihood > self.sem_gate:
if sem_likelihood > self.sem_gate:
errors_right.append(error)
# Measurement likelihood (based on noise cov)
meas_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=self.noise_cov)
# Geometric likelihood (based on innov)
geo_likelihood = multivariate_normal.pdf(
error.reshape(-1), cov=innov)
meas_likelihoods.append(meas_likelihood)
asso_prob = geo_likelihood * sem_likelihood
asso_probs.append(asso_prob)
else:
errors_right.append(None)
meas_likelihoods.append(0)
asso_probs.append(0)
asso_probs = np.asarray(asso_probs)
meas_likelihoods = np.asarray(meas_likelihoods)
# Compute weights based on total probability theorem
if asso_probs.sum():
weights_right = (1-self.prob_null) * \
(asso_probs/np.sum(asso_probs))
else:
weights_right = np.zeros(asso_probs.shape)
# Weight measurement likelihoods
weighted_meas_likelihood = weights_right*meas_likelihoods
# Add weight and weighted likelihood of null hypothesis
weights_right = np.insert(weights_right, 0, [0., self.prob_null])
weighted_meas_likelihood = np.insert(
weighted_meas_likelihood, 0, [0., null_weighted_meas_likelihood, ])
asso_table[1, :] = weighted_meas_likelihood
# GNN association
# This is performed at every optimization step, so this factor is essentially
# a max-mixture factor. It's just now the max operation is replaced by the
# linear sum assignment operation.
# Take log so the association result maximizes the product of likelihoods
# of the associations of the both sides
log_asso_table = np.log(asso_table)
_, col_idc = lsa(log_asso_table, maximize=True)
asso_idx_left = col_idc[0]
asso_idx_right = col_idc[1]
# Left assocation
if asso_idx_left == 0:
# Null hypothesis
self._null_hypo_left = True
else:
self._null_hypo_left = False
chosen_error_left = errors_left[asso_idx_left-2]
self.chosen_expected_coeffs_left = expected_coeffs_list[asso_idx_left-2]
self._scale_left = weights_left[asso_idx_left]
# Right assocation
if asso_idx_right == 1:
# Null hypothesis
self._null_hypo_right = True
else:
self._null_hypo_right = False
chosen_error_right = errors_right[asso_idx_right-2]
self.chosen_expected_coeffs_right = expected_coeffs_list[asso_idx_right-2]
self._scale_right = weights_right[asso_idx_right]
if self._null_hypo_left:
chosen_error_left = null_error
self.chosen_expected_coeffs_left = null_expected_c0c1_left
if self._null_hypo_right:
chosen_error_right = null_error
self.chosen_expected_coeffs_right = null_expected_c0c1_right
chosen_error = np.concatenate((chosen_error_left, chosen_error_right))
return chosen_error
# # Make constraints at each xk
# c_eq = np.zeros(N)
# for ii, xk in np.ndenumerate(x):
# c_eq[ii] = count(x, xk)
# Assuming we've guessed the right indices, can we find the right values?
c0 = np.ones(k)
res = basinhopping(obj, c0, minimizer_kwargs={'args': (
idx_true,
x,
)})
print(res)
print(res['x'], c_true[idx_true])
# Now solve the assignment problem
c = np.zeros(N)
c[idx_true] = res['x']
xhat = inverse(c)
C = cdist(xhat[:, None], x[:, None])
rows, cols = lsa(C)
# Show the people the coefficients!
plt.plot(-np.sort(-np.abs(forward(xhat))), label='xhat')
plt.plot(-np.sort(-np.abs(forward(x[cols]))), '--', label='An x_pi')
plt.plot(-np.sort(-np.abs(forward(np.sort(x)))), ':', label='sort(x)')
plt.legend()
plt.title('Sorted DCT Coefficients')
plt.show()
